def __init__(self): self.action_space = [ 'N', 'E', 'W', 'S', 'NE', 'NW', 'SE', 'SW', 'shoot' ] self.map = np.array(Image.open("icra2.pgm")) self.shoot_pub = rospy.Publisher('shoot_cmd', ShootCmd, queue_size=1) self.goal_pub = rospy.Publisher('move_base_simple/goal', PoseStamped, queue_size=1) self.count = 0 self.navgoal = PoseStamped() self.navgoal.header.frame_id = 'map' #self.navgoal.pose.orientation.w = 1.0 self.shoot = ShootCmd() self.MyPose = {'x': 0, 'y': 0, 'theta': 0} self.EnemyPose = EnemyPos() self.Gimbal = EnemyPos() self.enemyNew = False self.EnemyPoseSave = EnemyPos() self.totalhurt = [0, 0] self.lastpose = [4, 2.5, 0] self.lastgoal = [4, 2.5, 0] self.goal_x = 4 self.goal_y = 2.5 self.goal_yaw = 0 self.goingtopoint = False self.isturned = False self.goal_yawsave = 0 self.num = 0 self.getbuff = False self.sended = False self.gimbalYawSave = 0 rospy.init_node('BattleSim')
MANUAL_SEPARATE_GIMBAL = 2 MANUAL_FOLLOW_GIMBAL = 3 DODGE_MODE = 4 #底盘躲避模式,底盘固定旋转,平移不受控制 AUTO_SEPARATE_GIMBAL = 5 #底盘和云台分离模式,旋转、平移受上层控制查看tf tree AUTO_FOLLOW_GIMBAL = 6 # 底盘跟随云台,平移受上层控制 #启动控制器和环境类 controller = Controller() env = BattleEnv() plan = Plan() follow = Follow() tflistener = tf.TransformListener() #定义控制命令 vel = Twist() nav_goal = env.navgoal shoot_cmd = ShootCmd() mode = ModeSW() num1 = 31 #接受机器人状态消息 rospy.Subscriber('gimbalpos', EnemyPos, env.getGimbalPoseCallback) rospy.Subscriber('my_pose', Odometry, env.getSelfPoseCallback) time.sleep(0.1) rospy.Subscriber('/enemy_pos', EnemyPos, env.getEnemyPoseCallback) time.sleep(0.1) print 'orginal my pose is x=%s\ty=%s\t yaw=%s\n' % ( env.MyPose['x'], env.MyPose['y'], env.MyPose['theta']) print 'orginal enemy pose is yaw=%s\tpitch=%s\tdistance=%s\n' % ( env.EnemyPose.enemy_yaw, env.EnemyPose.enemy_pitch, env.EnemyPose.enemy_dist) # env.shooting_stop(shoot_cmd, controller)
x = 0 y = 0 z = 0 th = 0 status = 0 yaw = 0 pitch = 0 sc = 0 csc = 0 fwr = 0 fws = 1500 #定义三种消息类 twist = Twist() gimbal = EnemyPos() shoot = ShootCmd() try: #print teleop.msg print teleop.vels(speed, turn) print "gimbal step is: \t", gimbalstep print "yaw angle: %s\t\n pitch angle: %s\t\n" % (yaw, pitch) print "single shoot: %s\t continue shoot: %s\t gun on: %s\t gun speed: %s\t" % ( sc, csc, fwr, fws) while (1): key = teleop.getKey() #获取键盘按键 #key='i' if key in teleop.moveBindings.keys(): #速度按键信息解析 x = teleop.moveBindings[key][0] y = teleop.moveBindings[key][1] z = teleop.moveBindings[key][2]
def __init__(self): self.msg = """ Reading from the keyboard and Publishing to Twist! --------------------------- Moving around: u i o j k l m , . For Holonomic mode (strafing), hold down the shift key: --------------------------- U I O J K L M < > t : up (+z) b : down (-z) For RM Robot: gimbal control:+/-steps by 1 degree 8 4 6 2 shoot control: G-> GUN ON, T/B +/- fric wheel speed by 100 5->single shoot space->continue shoot 0->stop shoot anything else : stop q/z : increase/decrease max speeds by 10% w/x : increase/decrease only linear speed by 10% e/c : increase/decrease only angular speed by 10% CTRL-C to quit """ self.moveBindings = { 'i': (1, 0, 0, 0), 'o': (1, 0, 0, -1), 'j': (0, 0, 0, 1), 'l': (0, 0, 0, -1), 'u': (1, 0, 0, 1), ',': (-1, 0, 0, 0), '.': (-1, 0, 0, 1), 'm': (-1, 0, 0, -1), 'k': (0, 0, 0, 0), 'K': (0, 0, 0, 0), 'O': (1, -1, 0, 0), 'I': (1, 0, 0, 0), 'J': (0, 1, 0, 0), 'L': (0, -1, 0, 0), 'U': (1, 1, 0, 0), '<': (-1, 0, 0, 0), '>': (-1, -1, 0, 0), 'M': (-1, 1, 0, 0), # 't': (0, 0, 1, 0), # 'b': (0, 0, -1, 0), } self.speedBindings = { 'q': (1.1, 1.1, 0), 'z': (.9, .9, 0), 'w': (1.1, 1, 0), 'x': (.9, 1, 0), 'e': (1, 1.1, 0), 'c': (1, .9, 0), 'r': (0, 0, 1), 'v': (0, 0, -1), } self.gimbalBindings = { '8': (1, 0), '2': (-1, 0), '4': (0, 1), '6': (0, -1), } self.shootBindings = { '5': (1, 0, 1, 0), ' ': (0, 1, 1, 0), 'G': (0, 0, 1, 0), 'T': (0, 0, 1, 100), 'B': (0, 0, 1, -100), '0': (0, 0, 0, 0), } self.vel_pub = rospy.Publisher('robot_0/cmd_vel', Twist, queue_size=1) #发布云台指向,模拟视觉输出信息 self.gimbal_pub = rospy.Publisher('enemy_pos', EnemyPos, queue_size=1) #射击指令 self.shoot_pub = rospy.Publisher('shoot_cmd', ShootCmd, queue_size=1) #模式切换 self.modesw_pub = rospy.Publisher('switch_mode', ModeSW, queue_size=1) #发布目标点 self.nav_goal_pub = rospy.Publisher('move_base_simple/goal', PoseStamped, queue_size=1) self.shoot_cmd = ShootCmd()
#!/usr/bin/env python # -*- coding: utf-8 -*- import rospy from Battle import BattleEnv from teleop_control import Controller from teleop_controller.msg import EnemyPos, ShootCmd, ModeSW from pi_trees_ros.pi_trees_ros import * controller = Controller() env = BattleEnv() shoot_cmd = ShootCmd() if __name__ == '__main__': rospy.init_node('ShootEnemy') rospy.Subscriber('enemy_pos', EnemyPos, env.getEnemyPoseCallback) rospy.Subscriber('robot_1/enemy_pos', EnemyPos, env.getEnemyPoseCallback_robot1) # 开两机器人摩擦轮 shoot_cmd.fric_wheel_spd = 1225 shoot_cmd.fric_wheel_run = 1 shoot_cmd.c_shoot_cmd = 0 shoot_cmd.shoot_cmd = 0 controller.shoot(shoot_cmd) controller.shoot_robot1(shoot_cmd) print 'start' while 1: if env.EnemyPoseSave.enemy_dist > 2.2: env.shooting(shoot_cmd, controller) print 'shoot=================================' elif env.EnemyPoseSave.enemy_dist > 0 and env.EnemyPoseSave.enemy_dist <= 2.2: env.shooting_plus(shoot_cmd, controller)