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)