def __init__(self, rospackage_name, launch_file_name):
self._rospackage_name = rospackage_name
self._launch_file_name = launch_file_name
self._path_launch_file_name = None
self.rospack = rospkg.RosPack()
# Check Package Exists
try:
pkg_path = self.rospack.get_path(rospackage_name)
except rospkg.common.ResourceNotFound:
rospy.logwarn("Package NOT FOUND...")
# If the package was found then we launch
if pkg_path:
launch_dir = os.path.join(pkg_path, "launch")
self._path_launch_file_name = os.path.join(launch_dir,
launch_file_name)
self.uuid = roslaunch.rlutil.get_or_generate_uuid(None, False)
roslaunch.configure_logging(self.uuid)
self.launch = ROSLaunchParent(self.uuid,
[self._path_launch_file_name])
self.launch.start()
else:
assert False, ("No Package Path was found for ROS apckage ==>" +
str(self._rospackage_name))
def launch_cars(self):
launch_file_path = '/home/parallels/Documents/University/Robomaster/robo_2020_buff_ws/src/roborts_bringup/launch/'
launch_file_name = launch_file_path + 'roborts_stage2020.launch'
rviz_file_name = launch_file_path + 'rviz_robosim2020.launch'
launch_file_names_and_args = [(launch_file_name, ['__ns:=' + tmp_ns])
for tmp_ns in self.ns_names]
launch_file_names_and_args.append(rviz_file_name)
self.cars_launch_parents = ROSLaunchParent('start_from_python',
launch_file_names_and_args)
self.cars_launch_parents.start()
def play_using_ros(self, save_file_name='./records/record1.npy'):
self.parent = ROSLaunchParent('start_from_python', [],
is_core=True) # run_id can be any string
self.parent.start()
self.file_name = save_file_name
self.ns_names = ['blue1', 'red1', 'blue2', 'red2'
] ### {0:'blue1', 1:'red1', 2:'blue2', 3:'red2'}
self.new_cmds = np.zeros((4, 4))
self.init_pub_and_sub()
self.launch_cars()
rospy.spin()
def restart(self):
if self._path_launch_file_name == None:
assert False, ("No Package Path was found for ROS apckage ==>" +
str(self._rospackage_name))
else:
self.launch.shutdown()
#a double check before starting launch file again
self.uuid = roslaunch.rlutil.get_or_generate_uuid(None, False)
roslaunch.configure_logging(self.uuid)
self.launch = ROSLaunchParent(self.uuid,
[self._path_launch_file_name])
self.launch.start()
class TLClassifierTest(TestCase):
ROS_LAUNCH_PARENT = ROSLaunchParent("TLClassifierTest", [], is_core=True)
@classmethod
def setUpClass(cls):
"""TODO (Sergey Morozov): remove and use rostest or rosunit"""
# start roscore
cls.ROS_LAUNCH_PARENT.start()
@classmethod
def tearDownClass(cls):
"""TODO (Sergey Morozov): remove and use rostest or rosunit"""
# stop roscore
cls.ROS_LAUNCH_PARENT.shutdown()
def setUp(self):
TLClassifier.INSTANCE = None
TLClassifier.KNOWN_TRAFFIC_LIGHT_CLASSIFIERS = {}
TLClassifier.register_subclass("opencv")(OpenCVTLClassifier)
TLClassifier.register_subclass("ssd-sim")(SSDSimTLClassifier)
TLClassifier.register_subclass("ssd-real")(SSDRealTLClassifier)
TLClassifier.register_subclass("yolo-tiny")(YOLOTinyTLClassifier)
def tearDown(self):
TLClassifier.INSTANCE = None
TLClassifier.KNOWN_TRAFFIC_LIGHT_CLASSIFIERS = {}
TLClassifier.register_subclass("opencv")(OpenCVTLClassifier)
TLClassifier.register_subclass("ssd-sim")(SSDSimTLClassifier)
TLClassifier.register_subclass("ssd-real")(SSDRealTLClassifier)
TLClassifier.register_subclass("yolo-tiny")(YOLOTinyTLClassifier)
def test_get_instance_of(self):
instance = TLClassifier.get_instance_of("opencv")
self.assertIsInstance(instance, OpenCVTLClassifier)
self.assertEqual(4, len(TLClassifier.KNOWN_TRAFFIC_LIGHT_CLASSIFIERS))
def test_classify(self):
@TLClassifier.register_subclass('mock')
class MockTLClassifier(TLClassifier):
def __init__(self, is_debug):
super(MockTLClassifier, self).__init__(self.__class__.__name__,
is_debug)
def _classify(self, image):
return None, None
def get_state_count_threshold(self, last_state):
pass
rospy.init_node('test_tl_classifier', anonymous=True)
mock_instance = TLClassifier.get_instance_of('mock')
for i in range(20):
mock_instance.classify(None)
self.assertEqual(20, len(mock_instance._start_time_circular_buffer))
for i in range(200):
mock_instance.classify(None)
self.assertEqual(100, len(mock_instance._start_time_circular_buffer))
def __init__(self, path_to_bringup, shutdown_topic_name):
### get path to agv_v2_bringup.launch
self.path_to_bringup = path_to_bringup
### get shutdown topic's name
self.shutdown_topic_name = shutdown_topic_name
self.shutdown_count = 0
self.count_to_shutdown = 5
### node name for shutdown subscriber
self.node_name = 'wait_for_shutdown'
### create ROSLaunchParent instance for roscore and agv_v2_bringup
# self.roscore = ROSLaunchParent('core', [], is_core=True)
self.bringup = ROSLaunchParent('bringup', [self.path_to_bringup],
is_core=True)
### set this flag to True after self node killed
self.shutdown_finished = False
def _subtestTimeouts(self):
from roslaunch.parent import ROSLaunchParent
from roslaunch.server import ROSLaunchParentNode
import signal
import tempfile
from threading import Thread
pmon = self.pmon
pmon.start()
try:
# if there is a core up, we have to use its run id
run_id = get_param('/run_id')
except:
run_id = 'test-rl-parent-timeout-%s' % time.time()
rl_dir = rospkg.RosPack().get_path('roslaunch')
rl_file = os.path.join(rl_dir, 'resources', 'timeouts.launch')
sigint_timeout = 2
sigterm_timeout = 3
p = ROSLaunchParent(run_id, [rl_file],
is_core=False,
port=11312,
local_only=True,
sigint_timeout=sigint_timeout,
sigterm_timeout=sigterm_timeout)
p._load_config()
p.pm = pmon
p.server = ROSLaunchParentNode(p.config, pmon)
signal_log_file = os.path.join(tempfile.gettempdir(), "signal.log")
try:
os.remove(signal_log_file)
except OSError:
pass
def kill_launch(times):
time.sleep(3) # give it time to start
times.append(time.time())
p.shutdown()
times.append(time.time())
p.start()
times = []
t = Thread(target=kill_launch, args=(times, ))
t.start()
p.spin()
t.join()
before_stop_call_time, after_stop_call_time = times
signals = dict()
try:
with open(signal_log_file, 'r') as f:
lines = f.readlines()
for line in lines:
sig, timestamp = line.split(" ")
sig = int(sig)
timestamp = float(timestamp)
signals[sig] = timestamp
except IOError:
self.fail("Could not open %s" % signal_log_file)
self.assertSetEqual({signal.SIGINT, signal.SIGTERM},
set(signals.keys()))
self.assertAlmostEqual(before_stop_call_time,
signals[signal.SIGINT],
delta=1.0)
self.assertAlmostEqual(before_stop_call_time,
signals[signal.SIGTERM] - sigint_timeout,
delta=1)
self.assertAlmostEqual(before_stop_call_time,
after_stop_call_time - sigint_timeout -
sigterm_timeout,
delta=1)
def _subroslaunchParent(self):
from roslaunch.parent import ROSLaunchParent
pmon = self.pmon
try:
# if there is a core up, we have to use its run id
run_id = get_param('/run_id')
except:
run_id = 'test-rl-parent-%s' % time.time()
name = 'foo-bob'
server_uri = 'http://localhost:12345'
p = ROSLaunchParent(run_id, [],
is_core=True,
port=None,
local_only=False)
self.assertEquals(run_id, p.run_id)
self.assertEquals(True, p.is_core)
self.assertEquals(False, p.local_only)
rl_dir = rospkg.RosPack().get_path('roslaunch')
rl_file = os.path.join(rl_dir, 'resources', 'example.launch')
self.assert_(os.path.isfile(rl_file))
# validate load_config logic
p = ROSLaunchParent(run_id, [rl_file],
is_core=False,
port=None,
local_only=True)
self.assertEquals(run_id, p.run_id)
self.assertEquals(False, p.is_core)
self.assertEquals(True, p.local_only)
self.assert_(p.config is None)
p._load_config()
self.assert_(p.config is not None)
self.assert_(p.config.nodes)
# try again with port override
p = ROSLaunchParent(run_id, [rl_file],
is_core=False,
port=11312,
local_only=True)
self.assertEquals(11312, p.port)
self.assert_(p.config is None)
p._load_config()
# - make sure port got passed into master
_, port = rosgraph.network.parse_http_host_and_port(
p.config.master.uri)
self.assertEquals(11312, port)
# try again with bad file
p = ROSLaunchParent(run_id, ['non-existent-fake.launch'])
self.assert_(p.config is None)
try:
p._load_config()
self.fail("load config should have failed due to bad rl file")
except roslaunch.core.RLException:
pass
# try again with bad xml
rl_dir = rospkg.RosPack().get_path('roslaunch')
rl_file = os.path.join(rl_dir, 'test', 'xml',
'test-params-invalid-1.xml')
self.assert_(os.path.isfile(rl_file))
p = ROSLaunchParent(run_id, [rl_file])
self.assert_(p.config is None)
try:
p._load_config()
self.fail("load config should have failed due to bad rl file")
except roslaunch.core.RLException:
pass
# Mess around with internal repr to get code coverage on _init_runner/_init_remote
p = ROSLaunchParent(run_id, [],
is_core=False,
port=None,
local_only=True)
# no config, _init_runner/_init_remote/_start_server should fail
for m in ['_init_runner', '_init_remote', '_start_server']:
try:
getattr(p, m)()
self.fail('should have raised')
except roslaunch.core.RLException:
pass
# - initialize p.config
p.config = roslaunch.config.ROSLaunchConfig()
# no pm, _init_runner/_init_remote/_start_server should fail
for m in ['_init_runner', '_init_remote', '_start_server']:
try:
getattr(p, m)()
self.fail('should have raised')
except roslaunch.core.RLException:
pass
# - initialize p.pm
p.pm = pmon
for m in ['_init_runner', '_init_remote']:
try:
getattr(p, m)()
self.fail('should have raised')
except roslaunch.core.RLException:
pass
from roslaunch.server import ROSLaunchParentNode
p.server = ROSLaunchParentNode(p.config, pmon)
p._init_runner()
# roslaunch runner should be initialized
self.assert_(p.runner is not None)
# test _init_remote
p.local_only = True
p._init_remote()
p.local_only = False
# - this violates many abstractions to do this
def ftrue():
return True
p.config.has_remote_nodes = ftrue
p._init_remote()
self.assert_(p.remote_runner is not None)
self.failIf(pmon.is_shutdown)
p.shutdown()
self.assert_(pmon.is_shutdown)
class rmaics(object):
def __init__(self,
agent_num,
render=True,
sim_rate=20,
update_display_every_n_epoch=1):
self.car_num = agent_num
self.game = kernal(
car_num=agent_num,
render=render,
update_display_every_n_epoch=update_display_every_n_epoch)
self.sim_rate = sim_rate
self.g_map = self.game.get_map()
self.memory = []
self.is_game_start = False
self.x_shift = 0.2
self.y_shift = 0.2
def reset(self):
self.state = self.game.reset()
# state, object
self.obs = self.get_observation(self.state)
return self.obs
def step(self, actions):
state = self.game.step(actions)
obs = self.get_observation(state)
rewards = self.get_reward(state)
self.memory.append([self.obs, actions, rewards])
self.state = state
return obs, rewards, state.done, None
def get_observation(self, state):
# personalize your observation here
obs = state
return obs
def get_reward(self, state):
# personalize your reward here
rewards = None
return rewards
def play(self):
self.game.play()
def save_record(self, file):
self.game.save_record(file)
def init_pub_and_sub(self):
rospy.init_node('Simulator_node')
self.cmd_sub_blue1 = rospy.Subscriber(self.ns_names[0] + '/' +
'cmd_vel',
Twist,
callback=self.cmd_callback_blue1,
queue_size=1) ###TODO
self.cmd_sub_red1 = rospy.Subscriber(self.ns_names[1] + '/' +
'cmd_vel',
Twist,
callback=self.cmd_callback_red1,
queue_size=1)
self.cmd_sub_blue2 = rospy.Subscriber(self.ns_names[2] + '/' +
'cmd_vel',
Twist,
callback=self.cmd_callback_blue2,
queue_size=1)
self.cmd_sub_red2 = rospy.Subscriber(self.ns_names[3] + '/' +
'cmd_vel',
Twist,
callback=self.cmd_callback_red2,
queue_size=1)
self.start_signal_sub = rospy.Subscriber(
'/start_signal',
Int16,
callback=self.game_start_callback,
queue_size=1)
self.br = tf.TransformBroadcaster()
self.odom_pubs_ = [
rospy.Publisher(ns + '/' + 'odom', Odometry, queue_size=10)
for ns in self.ns_names
]
## 2019
self.enemy_pubs_ = [
rospy.Publisher(ns + '/' + 'goal', PoseStamped, queue_size=1)
for ns in self.ns_names
] ### for cmd line use, not suitable in codes.
self.gimbal_angle_pubs_ = [
rospy.Publisher(ns + '/' + 'cmd_gimbal_angle',
GimbalAngle,
queue_size=1) for ns in self.ns_names
] ### for gimbal control
self.game_status_pubs_ = [
rospy.Publisher(ns + '/' + 'game_status',
GameStatus,
queue_size=30) for ns in self.ns_names
]
self.game_result_pubs_ = [
rospy.Publisher(ns + '/' + 'game_result',
GameResult,
queue_size=30) for ns in self.ns_names
]
self.game_survival_pubs_ = [
rospy.Publisher(ns + '/' + 'game_survivor',
GameSurvivor,
queue_size=30) for ns in self.ns_names
]
self.bonus_status_pubs_ = [
rospy.Publisher(ns + '/' + 'field_bonus_status',
BonusStatus,
queue_size=30) for ns in self.ns_names
]
self.supplier_status_pubs_ = [
rospy.Publisher(ns + '/' + 'field_supplier_status',
SupplierStatus,
queue_size=30) for ns in self.ns_names
]
self.robot_status_pubs_ = [
rospy.Publisher(ns + '/' + 'robot_status',
RobotStatus,
queue_size=30) for ns in self.ns_names
]
self.robot_heat_pubs_ = [
rospy.Publisher(ns + '/' + 'robot_heat', RobotHeat, queue_size=30)
for ns in self.ns_names
]
self.robot_bonus_pubs_ = [
rospy.Publisher(ns + '/' + 'robot_bonus',
RobotBonus,
queue_size=30) for ns in self.ns_names
]
self.robot_damage_pubs_ = [
rospy.Publisher(ns + '/' + 'robot_damage',
RobotDamage,
queue_size=30) for ns in self.ns_names
]
self.robot_shoot_pubs_ = [
rospy.Publisher(ns + '/' + 'robot_shoot',
RobotShoot,
queue_size=30) for ns in self.ns_names
]
self.projectile_supply_pubs_ = [
rospy.Publisher(ns + '/' + 'projectile_supply',
ProjectileSupply,
queue_size=1) for ns in self.ns_names
]
self.partner_pubs_ = [
rospy.Publisher(ns + '/' + 'partner_msg',
PartnerInformation,
queue_size=1) for ns in self.ns_names
]
self.enemy_info_pubs_ = [
rospy.Publisher(ns + '/' + 'enemy_info',
EnemyInfoVector,
queue_size=1) for ns in self.ns_names
]
## 2020
self.buff_info_pubs_ = [
rospy.Publisher(ns + '/' + 'BuffInfo', BuffInfo, queue_size=1)
for ns in self.ns_names
]
self.robot_punish_pubs_ = [
rospy.Publisher(ns + '/' + 'RobotPunish',
RobotPunish,
queue_size=1) for ns in self.ns_names
]
self.bullet_status_pubs_ = [
rospy.Publisher(ns + '/' + 'BulletVacant',
BulletVacant,
queue_size=1) for ns in self.ns_names
]
rospy.Timer(rospy.Duration(1.0 / self.sim_rate), self.timer_callback)
rospy.on_shutdown(self.save_record_h) ###save game before shut down
def launch_cars(self):
launch_file_path = '/home/parallels/Documents/University/Robomaster/robo_2020_buff_ws/src/roborts_bringup/launch/'
launch_file_name = launch_file_path + 'roborts_stage2020.launch'
rviz_file_name = launch_file_path + 'rviz_robosim2020.launch'
launch_file_names_and_args = [(launch_file_name, ['__ns:=' + tmp_ns])
for tmp_ns in self.ns_names]
launch_file_names_and_args.append(rviz_file_name)
self.cars_launch_parents = ROSLaunchParent('start_from_python',
launch_file_names_and_args)
self.cars_launch_parents.start()
def game_start_callback(self, int16_msg):
if not self.is_game_start:
self.is_game_start = True
print('Game start!!!!!')
else:
self.is_game_start = False
print('Game paused!!!!!')
def timer_callback(self, event):
if self.is_game_start:
self.step_ros(self.new_cmds)
else:
self.publish_every_thing()
def save_record_h(self):
self.game.save_record(self.file_name)
print('saved this game in: ', self.file_name)
def process_cmd(self, twist, car_index):
### simple orders consist of only x,y,rotate speeds and auto_aim
self.new_cmds[car_index, 0] = twist.linear.x
self.new_cmds[car_index, 1] = -twist.linear.y ## TODO:up down reverse
self.new_cmds[car_index, 2] = -np.degrees(twist.angular.z)
self.new_cmds[car_index, 3] = 1
def cmd_callback_blue1(self, twist):
self.process_cmd(twist, 0)
def cmd_callback_red1(self, twist):
self.process_cmd(twist, 1)
def cmd_callback_blue2(self, twist):
self.process_cmd(twist, 2)
def cmd_callback_red2(self, twist):
self.process_cmd(twist, 3)
def step_ros(self, actions):
self.state = self.game.step_simple_control(actions)
self.publish_every_thing()
if self.state.done:
self.cars_launch_parents.shutdown()
self.parent.shutdown()
rospy.signal_shutdown('Game end') ###shut down ros node
def publish_every_thing(self):
### state(time, self.cars, buff_info, time <= 0, detect, vision)
time, cars, buff_info, done, detect, vision = self.state.time, self.state.agents, self.state.buff, self.state.done, self.state.detect, self.state.vision
# 2019
## Game state
game_state_msg = GameStatus()
if done:
game_state_msg.game_status = 5
else:
game_state_msg.game_status = 4
game_state_msg.remaining_time = time
for i in range(self.car_num):
self.game_status_pubs_[i].publish(game_state_msg)
## Game result
if done and self.car_num == 4:
game_result_msg = GameResult()
b1_hp = cars[0, 6]
r1_hp = cars[1, 6]
b2_hp = cars[2, 6]
r2_hp = cars[3, 6]
is_blue_done = ((b1_hp <= 0) and (b2_hp <= 0))
is_red_done = ((r1_hp <= 0) and (r2_hp <= 0))
if is_blue_done:
game_result_msg.result = 1
elif is_red_done:
game_result_msg.result = 2
else:
if b1_hp + b2_hp > r1_hp + r2_hp:
game_result_msg.result = 2
elif b1_hp + b2_hp < r1_hp + r2_hp:
game_result_msg.result = 1
else:
game_result_msg.result = 0
for i in range(self.car_num):
self.game_result_pubs_[i].publish(game_result_msg)
## Game survival
if self.car_num == 4:
game_survival_msg = GameSurvivor()
game_survival_msg.blue3 = cars[0, 6] > 0
game_survival_msg.red3 = cars[1, 6] > 0
game_survival_msg.blue4 = cars[2, 6] > 0
game_survival_msg.red4 = cars[3, 6] > 0
for i in range(self.car_num):
self.game_survival_pubs_[i].publish(game_survival_msg)
## Robot status
id_dict = {0: 13, 1: 3, 2: 14, 3: 4}
for i in range(self.car_num):
robot_status_msg = RobotStatus()
robot_status_msg.id = id_dict[i]
robot_status_msg.remain_hp = cars[i, 6]
robot_status_msg.max_hp = 2000
robot_status_msg.heat_cooling_limit = 360
robot_status_msg.heat_cooling_rate = 240 if cars[i,
6] < 400 else 120
self.robot_status_pubs_[i].publish(robot_status_msg)
## Robot heat
for i in range(self.car_num):
robot_heat_msg = RobotHeat()
robot_heat_msg.shooter_heat = cars[i, 5]
self.robot_heat_pubs_[i].publish(robot_heat_msg)
## Robot damage
damage_source_dict = {0: 0, 1: 3, 2: 2, 3: 1}
for i in range(self.car_num):
robot_damage_msg = RobotDamage()
robot_damage_msg.damage_type = 4 #default value, unattacked
robot_damage_msg.damage_source = 4 #default value, no armor
if cars[i, 5] > 240:
robot_damage_msg.damage_type = 2
if (cars[i, 11] != 4) and (
cars[i, 15] <= 100): #attacked during the last 0.5 second
robot_damage_msg.damage_type = 0
robot_damage_msg.damage_source = damage_source_dict[int(
cars[i, 11])]
self.robot_damage_pubs_[i].publish(robot_damage_msg)
## Partner information
### ROS coordinate, origin: left_down_point; x:right; y:up
### Simulator coordinate, origin:left_up_point; x:right; y:down
who_is_enemy = {0: [1, 3], 1: [0, 2], 2: [1, 3], 3: [0, 2]}
for i in range(self.car_num):
partner_info_msg = PartnerInformation()
partner_info_msg.header.stamp = rospy.Time.now()
tmp_vision = vision[i]
for enemy_ind in who_is_enemy[i]:
if tmp_vision[enemy_ind]:
partner_info_msg.enemy_detected = True
one_enemy_info = EnemyInfo()
one_enemy_info.num = 1
one_enemy_info.enemy_pos.pose.position.x = cars[
enemy_ind, 1] / 100.0 + self.x_shift ##pixel to meter
one_enemy_info.enemy_pos.pose.position.y = (448 - cars[
enemy_ind, 2]) / 100.0 + self.y_shift ##pixel to meter
yaw_at_ros = -np.radians(cars[enemy_ind,
3]) ## deg to radians
one_enemy_info.enemy_pos.pose.orientation.w = np.cos(
yaw_at_ros / 2.0)
one_enemy_info.enemy_pos.pose.orientation.z = np.sin(
yaw_at_ros / 2.0)
partner_info_msg.enemy_info.append(one_enemy_info)
partner_info_msg.partner_pose.pose.position.x = cars[
i, 1] / 100.0 + self.x_shift
partner_info_msg.partner_pose.pose.position.y = (
448 - cars[i, 2]) / 100.0 + self.y_shift
yaw_at_ros = -np.radians(cars[i, 3])
partner_info_msg.partner_pose.pose.orientation.w = np.cos(
yaw_at_ros / 2.0)
partner_info_msg.partner_pose.pose.orientation.z = np.sin(
yaw_at_ros / 2.0)
partner_info_msg.bullet_num = cars[i, 10]
self.partner_pubs_[i].publish(
partner_info_msg
) ##TODO:it seems that no need to publish partner_info from simulation
### send tf
tmp_translation = [
cars[i, 1] / 100.0 + self.x_shift,
(448 - cars[i, 2]) / 100.0 + self.y_shift, 0
]
tmp_rotation = [
0, 0, np.sin(yaw_at_ros / 2.0),
np.cos(yaw_at_ros / 2.0)
]
self.br.sendTransform([0, 0, 0], [0, 0, 0, 1], rospy.Time.now(),
self.ns_names[i] + '/odom', 'map')
self.br.sendTransform(tmp_translation, tmp_rotation,
rospy.Time.now(),
self.ns_names[i] + '/base_footprint',
self.ns_names[i] + '/odom')
self.br.sendTransform([0, 0, 0], [0, 0, 0, 1], rospy.Time.now(),
self.ns_names[i] + '/base_link',
self.ns_names[i] + '/base_footprint')
tmp_gimbal_translation = [0, 0, 0.15]
tmp_gimbal_rotation = [
0, 0,
np.cos(np.radians(-cars[i, 4]) / 2),
np.sin(np.radians(-cars[i, 4]) / 2)
]
self.br.sendTransform(tmp_gimbal_translation, tmp_gimbal_rotation,
rospy.Time.now(),
self.ns_names[i] + '/gimbal',
self.ns_names[i] + '/base_footprint')
odom_msg = Odometry()
odom_msg.header.frame_id = self.ns_names[i] + '/odom'
odom_msg.pose.pose.position = partner_info_msg.partner_pose.pose.position
odom_msg.pose.pose.orientation = partner_info_msg.partner_pose.pose.orientation
odom_msg.twist.twist.linear.x = self.new_cmds[i, 0]
odom_msg.twist.twist.linear.y = -self.new_cmds[i, 1]
odom_msg.twist.twist.angular.z = -np.radians(self.new_cmds[i, 2])
self.odom_pubs_[i].publish(odom_msg)
# self.br.sendTransform(tmp_translation, tmp_rotation, rospy.Time.now(), self.ns_names[i]+'/base_link', self.ns_names[i]+'/odom')
### send enemy_info
who_is_enemy = {0: [1, 3], 1: [0, 2], 2: [1, 3], 3: [0, 2]}
for i in range(self.car_num):
enemy_info_msg = EnemyInfoVector()
for enemy_ind in who_is_enemy[i]:
one_enemy_info = EnemyInfo()
one_enemy_info.num = 1
one_enemy_info.enemy_pos.header.frame_id = 'map'
one_enemy_info.enemy_pos.pose.position.x = cars[
enemy_ind, 1] / 100.0 + self.x_shift ##pixel to meter
one_enemy_info.enemy_pos.pose.position.y = (448 - cars[
enemy_ind, 2]) / 100.0 + self.y_shift ##pixel to meter
yaw_at_ros = -np.radians(cars[enemy_ind, 3]) ## deg to radians
one_enemy_info.enemy_pos.pose.orientation.w = np.cos(
yaw_at_ros / 2.0)
one_enemy_info.enemy_pos.pose.orientation.z = np.sin(
yaw_at_ros / 2.0)
enemy_info_msg.enemy_info.append(one_enemy_info)
self.enemy_info_pubs_[i].publish(enemy_info_msg)
# 2020
## Bullet vacant
for i in range(self.car_num):
bullet_vacant_msg = BulletVacant()
bullet_vacant_msg.bullet_vacant = (cars[i, 10] <= 0)
bullet_vacant_msg.remaining_number = cars[i, 10]
self.bullet_status_pubs_[i].publish(bullet_vacant_msg)
## Buff info
buff_info_msg = BuffInfo()
buff_info_msg.F1.type = int(buff_info[0, 0])
buff_info_msg.F1.used = False if buff_info[0, 1] else True
buff_info_msg.F2.type = int(buff_info[1, 0])
buff_info_msg.F2.used = False if buff_info[1, 1] else True
buff_info_msg.F3.type = int(buff_info[2, 0])
buff_info_msg.F3.used = False if buff_info[2, 1] else True
buff_info_msg.F4.type = int(buff_info[3, 0])
buff_info_msg.F4.used = False if buff_info[3, 1] else True
buff_info_msg.F5.type = int(buff_info[4, 0])
buff_info_msg.F5.used = False if buff_info[4, 1] else True
buff_info_msg.F6.type = int(buff_info[5, 0])
buff_info_msg.F6.used = False if buff_info[5, 1] else True
for i in range(self.car_num):
self.buff_info_pubs_[i].publish(buff_info_msg)
## Robot punish
for i in range(self.car_num):
robot_punish_msg = RobotPunish()
robot_punish_msg.type = cars[i, 8]
robot_punish_msg.remaining_time = cars[
i, 7] / 200.0 ##epoch to second
self.robot_punish_pubs_[i].publish(robot_punish_msg)
# Unuseful
## Robot shoot, Unused
robot_shoot_msg = RobotShoot()
robot_shoot_msg.frequency = 6
robot_shoot_msg.speed = 18
for i in range(self.car_num):
self.robot_shoot_pubs_[i].publish(robot_shoot_msg)
## Projectile_supply, Unused
projectile_supply_msg = ProjectileSupply()
projectile_supply_msg.number = 0
for i in range(self.car_num):
self.projectile_supply_pubs_[i].publish(projectile_supply_msg)
## Robot bonus, Unused
robot_bonus_msg = RobotBonus()
for i in range(self.car_num):
self.robot_bonus_pubs_[i].publish(robot_bonus_msg)
## Bonus status, Unused
bonus_status_msg = BonusStatus()
for i in range(self.car_num):
self.bonus_status_pubs_[i].publish(bonus_status_msg)
## Supplier status, Unused
supplier_status_msg = SupplierStatus()
for i in range(self.car_num):
self.supplier_status_pubs_[i].publish(supplier_status_msg)
def play_using_ros(self, save_file_name='./records/record1.npy'):
self.parent = ROSLaunchParent('start_from_python', [],
is_core=True) # run_id can be any string
self.parent.start()
self.file_name = save_file_name
self.ns_names = ['blue1', 'red1', 'blue2', 'red2'
] ### {0:'blue1', 1:'red1', 2:'blue2', 3:'red2'}
self.new_cmds = np.zeros((4, 4))
self.init_pub_and_sub()
self.launch_cars()
rospy.spin()
class core_bringup_manager():
def __init__(self, path_to_bringup, shutdown_topic_name):
### get path to agv_v2_bringup.launch
self.path_to_bringup = path_to_bringup
### get shutdown topic's name
self.shutdown_topic_name = shutdown_topic_name
self.shutdown_count = 0
self.count_to_shutdown = 5
### node name for shutdown subscriber
self.node_name = 'wait_for_shutdown'
### create ROSLaunchParent instance for roscore and agv_v2_bringup
# self.roscore = ROSLaunchParent('core', [], is_core=True)
self.bringup = ROSLaunchParent('bringup', [self.path_to_bringup],
is_core=True)
### set this flag to True after self node killed
self.shutdown_finished = False
### shutdown ros then shutdown computer
def shutdown_all(self):
self.shutdown_ros()
self.shutdown_computer()
### kill all rosnodes except self --> shutdown bringup --> shutdown roscore --> kill self node
def shutdown_ros(self):
self.sub.unregister()
neo = neopixel()
neo.execute()
print("---start killing nodes---")
nodes_list = rosnode.get_node_names()
nodes_list.remove('/' + self.node_name)
rosnode.kill_nodes(nodes_list)
print("---other nodes killed---")
self.bringup.shutdown()
print("---core shutdown---")
rosnode.kill_nodes(['/' + self.node_name])
print("---self node killed---")
self.shutdown_finished = True
# self.roscore.shutdown()
### function for shutdown computer
def shutdown_computer(self):
sudopass = "******"
command = "shutdown now"
p = os.system('echo %s|sudo -S %s' % (sudopass, command))
### callback for shutdown message (twist: /debug_val.linear.y)
def shutdown_callback_twist(self, data):
if (data.linear.y == 0):
self.shutdown_ros()
### callback for shutdown message (bool: /shutdown)
def shutdown_callback_bool(self, data):
if (data.data):
self.shutdown_ros()
def shutdown_callback_uint8(self, data):
if (data.data == 4):
self.shutdown_ros()
elif (data.data == 0):
self.shutdown_count += 1
if (self.shutdown_count >= self.count_to_shutdown):
self.shutdown_ros()
else:
self.shutdown_count = 0
### start roscore and bringup and then start node subscribe for shutdown message
def start_ros(self):
# self.roscore.start()
self.bringup.start()
sleep(5)
self.sub = rospy.Subscriber(self.shutdown_topic_name,
UInt8,
self.shutdown_callback_uint8,
queue_size=10)
rospy.init_node(self.node_name)
def _subtest_ROSLaunchParent(self):
from roslaunch.parent import ROSLaunchParent
pmon = self.pmon
import roslib.params
try:
# if there is a core up, we have to use its run id
run_id = roslib.params.get_param('/run_id')
except:
run_id = 'test-rl-parent-%s'%time.time()
name = 'foo-bob'
server_uri = 'http://localhost:12345'
p = ROSLaunchParent(run_id, [], is_core = True, port=None, local_only=False)
self.assertEquals(run_id, p.run_id)
self.assertEquals(True, p.is_core)
self.assertEquals(False, p.local_only)
import roslib.packages
rl_dir = roslib.packages.get_pkg_dir('roslaunch')
rl_file = os.path.join(rl_dir, 'example.launch')
self.assert_(os.path.isfile(rl_file))
# validate load_config logic
p = ROSLaunchParent(run_id, [rl_file], is_core = False, port=None, local_only=True)
self.assertEquals(run_id, p.run_id)
self.assertEquals(False, p.is_core)
self.assertEquals(True, p.local_only)
self.assert_(p.config is None)
p._load_config()
self.assert_(p.config is not None)
self.assert_(p.config.nodes)
# try again with port override
p = ROSLaunchParent(run_id, [rl_file], is_core = False, port=11312, local_only=True)
self.assertEquals(11312, p.port)
self.assert_(p.config is None)
p._load_config()
# - make sure port got passed into master
_, port = roslib.network.parse_http_host_and_port(p.config.master.uri)
self.assertEquals(11312, port)
# try again with bad file
p = ROSLaunchParent(run_id, ['non-existent-fake.launch'])
self.assert_(p.config is None)
try:
p._load_config()
self.fail("load config should have failed due to bad rl file")
except roslaunch.core.RLException: pass
# try again with bad xml
rl_dir = roslib.packages.get_pkg_dir('test_roslaunch')
rl_file = os.path.join(rl_dir, 'test', 'xml', 'test-params-invalid-1.xml')
self.assert_(os.path.isfile(rl_file))
p = ROSLaunchParent(run_id, [rl_file])
self.assert_(p.config is None)
try:
p._load_config()
self.fail("load config should have failed due to bad rl file")
except roslaunch.core.RLException: pass
# run an empty launch
if 0:
p = ROSLaunchParent(run_id, [], is_core = False, port=None, local_only=True)
self.assertEquals(run_id, p.run_id)
self.assertEquals(False, p.is_core)
self.assertEquals(True, p.local_only)
thread.start_new_thread(kill_parent, (p,))
p.start()
p.spin()
# Mess around with internal repr to get code coverage on _init_runner/_init_remote
p = ROSLaunchParent(run_id, [], is_core = False, port=None, local_only=True)
# no config, _init_runner/_init_remote/_start_server should fail
for m in ['_init_runner', '_init_remote', '_start_server']:
try:
getattr(p, m)()
self.fail('should have raised')
except roslaunch.core.RLException: pass
# - initialize p.config
p.config = roslaunch.config.ROSLaunchConfig()
# no pm, _init_runner/_init_remote/_start_server should fail
for m in ['_init_runner', '_init_remote', '_start_server']:
try:
getattr(p, m)()
self.fail('should have raised')
except roslaunch.core.RLException: pass
# - initialize p.pm
p.pm = pmon
for m in ['_init_runner', '_init_remote']:
try:
getattr(p, m)()
self.fail('should have raised')
except roslaunch.core.RLException: pass
from roslaunch.server import ROSLaunchParentNode
p.server = ROSLaunchParentNode(p.config, pmon)
p._init_runner()
# roslaunch runner should be initialized
self.assert_(p.runner is not None)
# test _init_remote
p.local_only = True
p._init_remote()
p.local_only = False
# - this violates many abstractions to do this
def ftrue():
return True
p.config.has_remote_nodes = ftrue
p._init_remote()
self.assert_(p.remote_runner is not None)
self.failIf(pmon.is_shutdown)
p.shutdown()
self.assert_(pmon.is_shutdown)
def core_start():
parent = ROSLaunchParent('core', [], is_core=True)
parent.start()