cmdvel_stop.linear.y = 0
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)
GIMBAL_FOLLOW_ZGYRO = 3 #云台跟随底盘模式
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)
if rospy.Time.now().secs - self.time1 > 2.5:
self.blackboard.sended_patrol = False
self.blackboard.patrol_flag = self.blackboard.patrol_flag + 1
if self.blackboard.patrol_flag == 2:
self.blackboard.patrol_flag = 0
print 'ptl end!!!!!!!!!!!!!!!!!!!!!!!!'
return TaskStatus.SUCCESS
def reset(self):
pass
if __name__ == '__main__':
rospy.init_node('RM_Pre')
# 启动控制器和环境类
controller = Controller()
env = BattleEnv()
tflistener = tf.TransformListener()
# 接受机器人状态消息
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)
rospy.Subscriber('robot_1/enemy_pos', EnemyPos,
if rospy.Time.now().secs - self.time1 > 3:
self.blackboard.sended_patrol = False
self.blackboard.patrol_flag = self.blackboard.patrol_flag + 1
if self.blackboard.patrol_flag == 2:
self.blackboard.patrol_flag = 0
print 'ptl end!!!!!!!!!!!!!!!!!!!!!!!!'
return TaskStatus.SUCCESS
def reset(self):
pass
if __name__ == '__main__':
rospy.init_node('RM_robot1_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)
GIMBAL_FOLLOW_ZGYRO = 3 #云台跟随底盘模式 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) #接受机器人状态消息
#!/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)
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