elif key in teleop.gimbalBindings.keys(): #云台角度指令解析
yaw = yaw + teleop.gimbalBindings[key][1] * gimbalstep
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
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)
env.shooting_stop(shoot_cmd, controller)
print 'stop!!!!!!!!!!!!!!!!!!!!!!!!!!!!!1!!'