def create_dynamic_traffic_history_mission(
self, veh_id: str, trigger_time: float,
positional_radius: int) -> Tuple[Mission, Mission]:
"""Builds a vehicle out of
Args:
veh_id:
The id of a vehicle in the traffic history dataset.
trigger_time:
The time that this mission should become active.
positional_radius:
The goal radius for the positional goal.
Returns:
(positional_mission, traverse_mission): A positional mission that follows the initial
original vehicle's travel as well as a traverse style mission which is done when the
vehicle leaves the map.
"""
start, speed = self.get_vehicle_start_at_time(veh_id, trigger_time)
veh_goal = self._get_vehicle_goal(veh_id)
entry_tactic = default_entry_tactic(speed)
# create a positional mission and a traverse mission
positional_mission = Mission(
start=start,
entry_tactic=entry_tactic,
start_time=0,
goal=PositionalGoal(veh_goal, radius=positional_radius),
)
traverse_mission = Mission(
start=start,
entry_tactic=entry_tactic,
start_time=0,
goal=TraverseGoal(self._road_map),
)
return positional_mission, traverse_mission
def _agent_spec_callback(self, ros_agent_spec: AgentSpec):
assert (len(ros_agent_spec.tasks) == 1
), "more than 1 task per agent is not yet supported"
task = ros_agent_spec.tasks[0]
task_params = json.loads(task.params_json) if task.params_json else {}
task_version = task.task_ver or "latest"
agent_locator = f"{self._zoo_module}:{task.task_ref}-{task_version}"
agent_spec = None
try:
agent_spec = registry.make(agent_locator, **task_params)
except ImportError as ie:
rospy.logerr(
f"Unable to locate agent with locator={agent_locator}: {ie}")
if not agent_spec:
rospy.logwarn(
f"got unknown task_ref '{task.task_ref}' in AgentSpec message with params='{task.param_json}'. ignoring."
)
return
if (ros_agent_spec.end_pose.position.x != 0.0
or ros_agent_spec.end_pose.position.y != 0.0):
goal = PositionalGoal(
(
ros_agent_spec.end_pose.position.x,
ros_agent_spec.end_pose.position.y,
),
ros_agent_spec.veh_length,
)
else:
goal = EndlessGoal()
mission = Mission(
start=Start.from_pose(
ROSDriver._pose_from_ros(ros_agent_spec.start_pose)),
goal=goal,
# TODO: how to prevent them from spawning on top of another existing vehicle? (see how it's done in SUMO traffic)
entry_tactic=default_entry_tactic(ros_agent_spec.start_speed),
vehicle_spec=VehicleSpec(
veh_id=f"veh_for_agent_{ros_agent_spec.agent_id}",
veh_config_type=ROSDriver._decode_vehicle_type(
ros_agent_spec.veh_type),
dimensions=Dimensions(
ros_agent_spec.veh_length,
ros_agent_spec.veh_width,
ros_agent_spec.veh_height,
),
),
)
with self._reset_lock:
if (ros_agent_spec.agent_id in self._agents
or ros_agent_spec.agent_id in self._agents_to_add):
rospy.logwarn(
f"trying to add new agent with existing agent_id '{ros_agent_spec.agent_id}'. ignoring."
)
return
self._agents_to_add[ros_agent_spec.agent_id] = (agent_spec,
mission)
def add_trap_for_agent(self, agent_id: str, mission: Mission,
road_map) -> bool:
"""Add a new trap to capture an actor for the given agent."""
if mission is None:
mission = Mission.random_endless_mission(road_map)
if not mission.entry_tactic:
mission = replace(mission, entry_tactic=default_entry_tactic())
if (not isinstance(mission.entry_tactic, TrapEntryTactic)
and mission.entry_tactic):
return False
plan = Plan(road_map, mission)
trap = self._mission2trap(road_map, plan.mission)
self._traps[agent_id] = trap
return True
def discover_missions_of_traffic_histories(self) -> Dict[str, Mission]:
"""Retrieves the missions of traffic history vehicles."""
vehicle_missions = {}
for row in self._traffic_history.first_seen_times():
v_id = str(row[0])
start_time = float(row[1])
start, speed = self.get_vehicle_start_at_time(v_id, start_time)
entry_tactic = default_entry_tactic(speed)
veh_config_type = self._traffic_history.vehicle_config_type(v_id)
veh_dims = self._traffic_history.vehicle_dims(v_id)
vehicle_missions[v_id] = Mission(
start=start,
entry_tactic=entry_tactic,
goal=TraverseGoal(self.road_map),
start_time=start_time,
vehicle_spec=VehicleSpec(
veh_id=v_id,
veh_config_type=veh_config_type,
dimensions=veh_dims,
),
)
return vehicle_missions