def maps():
outdir = get_comptests_output_dir()
map_names = list_maps()
print(map_names)
for map_name in map_names:
duckietown_map = load_map(map_name)
out = os.path.join(outdir, map_name)
draw_map(out, duckietown_map)
def _create_scenarios(self, context: Context):
available = list_maps()
for map_name in self.config.maps:
if not map_name in available:
msg = f'Cannot find map name "{map_name}, know {available}'
raise Exception(msg)
s: str = _get_map_yaml(map_name)
# yaml_str = update_map(yaml_str)
yaml_data = yaml.load(s, Loader=yaml.SafeLoader)
update_map(yaml_data)
yaml_str = yaml.dump(yaml_data)
po = dw.construct_map(yaml_data)
for imap in range(self.config.scenarios_per_map):
scenario_name = f'{map_name}-{imap}'
nrobots = self.config.robots_npcs + self.config.robots_pcs
poses = sample_many_good_starting_poses(po, nrobots,
only_straight=self.config.only_straight,
min_dist=self.config.min_dist,
delta_theta_rad=np.deg2rad(self.config.theta_tol_deg),
delta_y_m=self.config.dist_tol_m)
poses_pcs = poses[:self.config.robots_pcs]
poses_npcs = poses[self.config.robots_pcs:]
robots = {}
for i in range(self.config.robots_pcs):
pose = poses_pcs[i]
vel = g.se2_from_linear_angular([0, 0], 0)
robot_name = 'ego' if i == 0 else "player%d" % i
configuration = RobotConfiguration(pose=pose, velocity=vel)
robots[robot_name] = ScenarioRobotSpec(description='Playable robot',
playable=True,
configuration=configuration)
for i in range(self.config.robots_npcs):
pose = poses_npcs[i]
vel = g.se2_from_linear_angular([0, 0], 0)
robot_name = "npc%d" % i
configuration = RobotConfiguration(pose=pose, velocity=vel)
robots[robot_name] = ScenarioRobotSpec(description='NPC robot',
playable=False,
configuration=configuration)
ms = MyScenario(scenario_name=scenario_name, environment=yaml_str, robots=robots)
self.state.scenarios_to_go.append(ms)
def _create_scenarios(self, context: Context):
available = list_maps()
for map_name in self.config.maps:
if not map_name in available:
msg = f'Cannot find map name "{map_name}, know {available}'
raise Exception(msg)
yaml_str: str = _get_map_yaml(map_name)
po = load_map(map_name)
for i in range(self.config.scenarios_per_map):
scenario_name = f'{map_name}-{i}'
nrobots = self.config.robots_npcs + self.config.robots_pcs
poses = sample_many_good_starting_poses(
po,
nrobots,
only_straight=self.config.only_straight,
min_dist=self.config.min_dist)
poses_pcs = poses[:self.config.robots_pcs]
poses_npcs = poses[self.config.robots_pcs:]
robots = {}
for i in range(self.config.robots_pcs):
pose = poses_pcs[i]
vel = geometry.se2_from_linear_angular([0, 0], 0)
robot_name = 'ego' if i == 0 else "player%d" % i
configuration = RobotConfiguration(pose=pose, velocity=vel)
robots[robot_name] = ScenarioRobotSpec(
description='Playable robot',
playable=True,
configuration=configuration)
for i in range(self.config.robots_npcs):
pose = poses_npcs[i]
vel = geometry.se2_from_linear_angular([0, 0], 0)
robot_name = "npc%d" % i
configuration = RobotConfiguration(pose=pose, velocity=vel)
robots[robot_name] = ScenarioRobotSpec(
description='NPC robot',
playable=False,
configuration=configuration)
ms = MyScenario(scenario_name=scenario_name,
environment=yaml_str,
robots=robots)
self.state.scenarios_to_go.append(ms)
def wb2():
root = PlacedObject()
for map_name in list_maps():
tm = load_map(map_name)
root.set_object(map_name, tm)
d = root.as_json_dict()
# print(json.dumps(d, indent=4))
# print(yaml.safe_dump(d, default_flow_style=False))
# print('------')
r1 = Serializable.from_json_dict(d)
d1 = r1.as_json_dict()
def __init__(self):
super(DistributedTFNode, self).__init__(
node_name='distributed_tf_node',
node_type=NodeType.SWARM
)
# get static parameter - `~veh`
try:
self.robot_hostname = rospy.get_param('~veh')
except KeyError:
self.logerr('The parameter ~veh was not set, the node will abort.')
exit(1)
# get static parameter - `~map`
try:
self.map_name = rospy.get_param('~map')
except KeyError:
self.logerr('The parameter ~map was not set, the node will abort.')
exit(2)
# make sure the map exists
maps = dw.list_maps()
if self.map_name not in maps:
self.logerr(f"Map `{self.map_name}` not found in "
f"duckietown-world=={dw.__version__}. "
f"The node will abort.")
exit(2)
self._map = dw.load_map(self.map_name)
# create communication group
self._group = DTCommunicationGroup("/autolab/tf", AutolabTransform)
# create TF between the /world frame and the origin of this map
self._world_to_map_origin_tf = AutolabTransform(
origin=AutolabReferenceFrame(
time=rospy.Time.now(),
type=AutolabReferenceFrame.TYPE_WORLD,
name="world",
robot="*"
),
target=AutolabReferenceFrame(
time=rospy.Time.now(),
type=AutolabReferenceFrame.TYPE_MAP_ORIGIN,
name="map",
robot=self.robot_hostname
),
is_fixed=True,
is_static=False,
transform=Transform(
translation=Vector3(x=0, y=0, z=0),
rotation=Quaternion(x=0, y=0, z=0, w=1)
)
)
# create static tfs holder and access semaphore
self._static_tfs = [
self._world_to_map_origin_tf
]
# add TFs from ground tags
self._static_tfs.extend(self._get_tags_tfs())
# create publisher
self._tf_pub = self._group.Publisher()
# publish right away and then set a timer
self._publish_tfs()
self._pub_timer = rospy.Timer(
rospy.Duration(self.PUBLISH_TF_STATIC_EVERY_SECS),
self._publish_tfs
)
