GIMBAL_TRACK_ARMOR = 4 # 云台追踪装甲，icra 不使用
GIMBAL_PATROL_MODE = 5 #巡逻模式，云台 yaw 周期运动，pitch 不受控制
GIMBAL_SHOOT_BUFF = 6 #打大符模式，icra 不使用
GIMBAL_POSITION_MODE = 7 #云台位置模式，上层控制角度两轴角度
CHASSIS_RELAX = 0
CHASSIS_STOP = 1
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)
GIMBAL_TRACK_ARMOR = 4 # 云台追踪装甲，icra 不使用
GIMBAL_PATROL_MODE = 5 #巡逻模式，云台 yaw 周期运动，pitch 不受控制
GIMBAL_SHOOT_BUFF = 6 #打大符模式，icra 不使用
GIMBAL_POSITION_MODE = 7 #云台位置模式，上层控制角度两轴角度
CHASSIS_RELAX = 0
CHASSIS_STOP = 1
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()
tflistener = tf.TransformListener()
#定义控制命令
vel = Twist()
nav_goal = env.navgoal
shoot_cmd = ShootCmd()
mode = ModeSW()
controller.shoot_cmd.fric_wheel_spd = 1215
controller.shoot_cmd.fric_wheel_run = 1
controller.shoot_cmd.c_shoot_cmd = 0
controller.shoot_cmd.shoot_cmd = 0
controller.shoot(controller.shoot_cmd)
#接受机器人状态消息
rospy.Subscriber('gimbalpos', EnemyPos, env.getGimbalPoseCallback)
cmdvel_stop.linear.z = 0
cmdvel_stop.angular.x = 0
cmdvel_stop.angular.y = 0
cmdvel_stop.angular.z = 0
# 推车命令
cmdvel_blockedpush = Twist()
cmdvel_blockedretreat = Twist()
# for test
buffiscutted = False
if __name__ == '__main__':
rospy.init_node('RM_Pre')
# 启动控制器和环境类
controller = Controller()
env = BattleEnv()
tflistener = tf.TransformListener()
# 标志位
time1 = 0 # 记录进入buff后的时间
time2 = 0 # 记录执行时间
# 接受机器人状态消息
rospy.Subscriber('gimbalpos', EnemyPos, env.getGimbalPoseCallback)
rospy.Subscriber('robot_1/gimbalpos', EnemyPos, env.getGimbalPoseCallback_robot1)
rospy.Subscriber('my_pose', Odometry, env.getSelfPoseCallback)
rospy.Subscriber('robot_1/my_pose', Odometry, env.getSelfPoseCallback_robot1)
# rospy.Subscriber('/robot_1/base_pose_ground_truth', Odometry, env.getSelfPoseCallback_robot1)
rospy.Subscriber('enemy_pos', EnemyPos, env.getEnemyPoseCallback)
#!/usr/bin/env python
# -*- coding: UTF-8 -*-
import tf
import rospy
import math
import numpy as np
from teleop_controller.msg import EnemyPos
from PIL import Image
from nav_msgs.msg import Odometry
from Battle import BattleEnv
envdetect = BattleEnv()
EnemyPose_robot2 = {'x': 0, 'y': 0, 'theta': 0}
EnemyPose_robot3 = {'x': 0, 'y': 0, 'theta': 0}
rospy.Subscriber('my_pose', Odometry, envdetect.getSelfPoseCallback)
# rospy.Subscriber('robot_1/my_pose', Odometry, envdetect.getSelfPoseCallback_robot1)
rospy.Subscriber('robot_1/my_pose', Odometry,
envdetect.getSelfPoseCallback_robot1)
def getEnemyPoseCallback_robot2(data):
EnemyPose_robot2['x'] = data.pose.pose.position.x
EnemyPose_robot2['y'] = data.pose.pose.position.y
qx = data.pose.pose.orientation.x
qy = data.pose.pose.orientation.y
qz = data.pose.pose.orientation.z
qw = data.pose.pose.orientation.w
angle = math.atan2(2 * (qx * qy + qz * qw),
qw * qw + qx * qx - qy * qy - qz * qz) * 180 / 3.1415926
EnemyPose_robot2['theta'] = angle
GIMBAL_TRACK_ARMOR = 4 # 云台追踪装甲，icra 不使用
GIMBAL_PATROL_MODE = 5 #巡逻模式，云台 yaw 周期运动，pitch 不受控制
GIMBAL_SHOOT_BUFF = 6 #打大符模式，icra 不使用
GIMBAL_POSITION_MODE = 7 #云台位置模式，上层控制角度两轴角度
CHASSIS_RELAX = 0
CHASSIS_STOP = 1
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()
num = 0
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)
#!/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)
#!/usr/bin/env python
# -*- coding: utf-8 -*-
import roslib
import rospy
import math
import tf
from Battle import BattleEnv
from teleop_control import Controller
import time
import numpy as np
controller = Controller()
env = BattleEnv()
if __name__ == '__main__':
rospy.init_node('sendgoal')
if env.isActionAvaliable(1, 2, 50):
controller.send_goal(env.navgoal)
print 'go to position x =%s y=%s'
else:
pass
while True:
hhh = 1
import rospy
from Battle import BattleEnv
from Patrol import Plan
from Following import Follow
from teleop_control import Controller
from geometry_msgs.msg import Twist, PoseStamped
from nav_msgs.msg import Odometry
from teleop_controller.msg import ShootCmd, ModeSW
import time
import math
import numpy as np
import tf
#启动控制器和环境类
controller = Controller()
env = BattleEnv()
plan = Plan()
follow = Follow()
tflistener = tf.TransformListener()
#定义控制命令
vel = Twist()
nav_goal = env.navgoal
shoot_cmd = ShootCmd()
mode = ModeSW()
# def correct():
#
# pub = rospy.Publisher('correct_location',Odometry, queue_size=10)
# rospy.init_node('correct', anonymous=True)
# rate = rospy.Rate(10) # 10hz
# while not rospy.is_shutdown():