def __init__(self, map_name):
self.node_name = rospy.get_name()
# Map
self.map = dw.load_map(map_name)
self.skeleton_graph = dw.get_skeleton_graph(self.map['tilemap'])
# Dispatcher
self.dispatcher = dispatcher.Dispatcher(self.skeleton_graph)
self.state = {'seq': 0, 'duckies': {}, 'requests': []}
# Subscribers
self.sub_paths = rospy.Subscriber('/flock_simulator/state',
FlockState,
self.cbState,
queue_size=1)
# Publishers
self.pub_commands = rospy.Publisher('/flock_simulator/commands',
FlockCommand,
queue_size=1)
# Timer
self.sim_frequency = 30.0 # Frequency of simulation in Hz
self.request_timer = rospy.Timer(
rospy.Duration.from_sec(1.0 / self.sim_frequency), self.cbTimer)
self.isUpdating = False
def __init__(self, map_name):
self.map = dw.load_map(map_name)
self.tile_size = self.map.tile_size
self.skeleton_graph = dw.get_skeleton_graph(self.map['tilemap'])
self.graph = self.skeleton_graph.G
self.edges = list(self.graph.edges(data=True))
self.nodes = list(self.graph.nodes(data=True))
self.lanes = self.skeleton_graph.root2.children
def get_lane_at_point_test2():
m: dw.DuckietownMap = dw.load_map("robotarium2")
sk2 = dw.get_skeleton_graph(m)
G0 = sk2.G0
c = list(connected_components(G0.to_undirected()))
assert len(c) == 6
p = list(G0.nodes)[0]
mp: MeetingPoint = G0.nodes[p]["meeting_point"]
print(debug_print(mp))
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 test_pose_sampling_1():
m = dw.load_map("4way")
along_lane = 0.2
only_straight = True
for i in range(45):
q = sample_good_starting_pose(m,
only_straight=only_straight,
along_lane=along_lane)
ground_truth = dw.SE2Transform.from_SE2(q)
m.set_object(f"sampled-{i}", dw.DB18(), ground_truth=ground_truth)
outdir = get_comptests_output_dir()
draw_static(m, outdir)
def gltf_export_main(args: Optional[List[str]] = None):
parser = argparse.ArgumentParser()
parser.add_argument(
"--map", type=str,
)
parser.add_argument("--output", type=str)
parsed = parser.parse_args(args=args)
# ---
dm: DuckietownMap = load_map(parsed.map)
export_gltf(dm, parsed.output)
def get_lane_at_point_test():
m: dw.DuckietownMap = dw.load_map("robotarium2")
p = geo.SE2_from_translation_angle([1.3, 0.3], 0.2)
r = get_tile_at_point(m, p)
assert r.i, r.j == (2, 0)
This script outputs a 'good' starting position and angle for an agent, given a map. That means, agent starts at a point
that is close to the center of a lane and starts at an angle that is close to zero, which means agent is aligned with
the lane.
"""
from duckietown_world.world_duckietown.sampling_poses import sample_good_starting_pose
import duckietown_world as dw
import geometry as geo
import numpy as np
import argparse
# construct the argument parser and parse the arguments
ap = argparse.ArgumentParser()
ap.add_argument("-m", "--map-name", required=True, type=str,
help="map name as in maps/ folder")
args = vars(ap.parse_args())
map_name = args["map_name"]
along_lane = 1
only_straight = True
m = dw.load_map(map_name)
q = sample_good_starting_pose(m, only_straight=only_straight, along_lane=along_lane)
translation, angle = geo.translation_angle_from_SE2(q)
propose_pos = np.array([translation[0], 0, translation[1]])
propose_angle = angle
print(f"\nReturning for the map: '{map_name}'\n"
f"Pose: {propose_pos}",
f"\t|\tAngle: {propose_angle}")
# disabling contracts for speed
import contracts
import yaml
import geometry as geo
import numpy as np
from duckietown_world.world_duckietown.tile import get_lane_poses
import duckietown_world as dw
import geometry as g
import pandas as pd
contracts.disable_all()
# Load Duckietown map
m = dw.load_map('robotarium2')
# Folders that should be processed
folderNames = ['autobot04_r2']
# Calculates relative pose between two poses
def relative_pose(q0, q1):
return g.SE2.multiply(g.SE2.inverse(q0), q1)
# Interpolates position based on timestamp between two poses
def interpolate(q0, q1, alpha):
q1_from_q0 = relative_pose(q0, q1)
vel = g.SE2.algebra_from_group(q1_from_q0)
rel = g.SE2.group_from_algebra(vel * alpha)
q = g.SE2.multiply(q0, rel)
return q
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
)
