pitch = pitch + teleop.gimbalBindings[key][0] * gimbalstep gimbal.enemy_yaw = yaw #*3.1416/180 gimbal.enemy_pitch = pitch #*3.1416/180 gimbal.enemy_dist = 2.0 teleop.gimbal_pub.publish(gimbal) print "yaw angle: %s\t pitch angle: %s\t" % (yaw, pitch) elif key in teleop.shootBindings.keys(): #射击指令解析 sc = teleop.shootBindings[key][0] csc = teleop.shootBindings[key][1] fwr = teleop.shootBindings[key][2] fws = fws + teleop.shootBindings[key][3] shoot.shoot_cmd = sc shoot.c_shoot_cmd = csc shoot.fric_wheel_run = fwr shoot.fric_wheel_spd = fws teleop.shoot_pub.publish(shoot) print "single shoot: %s\t continue shoot: %s\t gun on: %s\t gun speed: %s\t" % ( sc, csc, fwr, fws) else: x = 0 y = 0 z = 0 th = 0 yaw = 0 pitch = 0 sc = 0 csc = 0 fwr = 0 fws = 0
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) print 'shoot++++++++++++++++++++++++++++++++' else: env.shooting_stop(shoot_cmd, controller)