def __init__(self):
"""
Constructor
"""
super(CarlaAdAgent, self).__init__('carla_ad_agent')
role_name = self.get_param("role_name", "ego_vehicle")
avoid_risk = self.get_param("avoid_risk", True)
self.target_speed = self.get_param("target_speed", 30.0)
self.agent = None
# wait for ego vehicle
vehicle_info = None
self.loginfo("Wait for vehicle info ...")
vehicle_info = self.wait_for_one_message(
"/carla/{}/vehicle_info".format(role_name),
CarlaEgoVehicleInfo,
qos_profile=QoSProfile(depth=1, durability=latch_on))
self.loginfo("Vehicle info received.")
self.agent = BasicAgent(role_name, vehicle_info.id, self, avoid_risk)
self.target_speed_subscriber = self.create_subscriber(
Float64, "/carla/{}/target_speed".format(role_name),
self.target_speed_updated, QoSProfile(depth=1, durability=True))
self.speed_command_publisher = self.new_publisher(
Float64, "/carla/{}/speed_command".format(role_name),
QoSProfile(depth=1, durability=True))
def __init__(self, role_name, target_speed, avoid_risk):
"""
Constructor
"""
self._route_assigned = False
self._global_plan = None
self._agent = None
self._target_speed = target_speed
rospy.on_shutdown(self.on_shutdown)
# wait for ego vehicle
vehicle_info = None
try:
vehicle_info = rospy.wait_for_message(
"/carla/{}/vehicle_info".format(role_name), CarlaEgoVehicleInfo)
except rospy.ROSException:
rospy.logerr("Timeout while waiting for world info!")
sys.exit(1)
self._route_subscriber = rospy.Subscriber(
"/carla/{}/waypoints".format(role_name), Path, self.path_updated)
self._target_speed_subscriber = rospy.Subscriber(
"/carla/{}/target_speed".format(role_name), Float64, self.target_speed_updated)
self.vehicle_control_publisher = rospy.Publisher(
"/carla/{}/vehicle_control_cmd".format(role_name), CarlaEgoVehicleControl, queue_size=1)
self._agent = BasicAgent(role_name, vehicle_info.id, # pylint: disable=no-member
avoid_risk)
def __init__(self, _name, _sensors=[]):
BasicAgent.__init__(self, _name)
self.sensors = _sensors
self.subscribers = {}
for s in self.sensors:
self.subscribers[s] = rospy.Subscriber(self.name, String, self.cb_setvar)
def test_merge_adjacent_cells(self):
state = NumbersState.from_table(
[[2, 8, 32, 2],
[0, 16, 64, 8],
[2, 32, 256, 16],
[4, 2, 32, 8]])
agent = BasicAgent(BasicHeuristic())
self.assertEqual(UP, agent.solve(state, 1))
class CarlaAdAgent(CompatibleNode):
"""
A basic AD agent using CARLA waypoints
"""
def __init__(self):
"""
Constructor
"""
super(CarlaAdAgent, self).__init__('carla_ad_agent')
role_name = self.get_param("role_name", "ego_vehicle")
avoid_risk = self.get_param("avoid_risk", True)
self.target_speed = self.get_param("target_speed", 30.0)
self.agent = None
# wait for ego vehicle
vehicle_info = None
self.loginfo("Wait for vehicle info ...")
vehicle_info = self.wait_for_one_message(
"/carla/{}/vehicle_info".format(role_name),
CarlaEgoVehicleInfo,
qos_profile=QoSProfile(depth=1, durability=latch_on))
self.loginfo("Vehicle info received.")
self.agent = BasicAgent(role_name, vehicle_info.id, self, avoid_risk)
self.target_speed_subscriber = self.create_subscriber(
Float64, "/carla/{}/target_speed".format(role_name),
self.target_speed_updated, QoSProfile(depth=1, durability=True))
self.speed_command_publisher = self.new_publisher(
Float64, "/carla/{}/speed_command".format(role_name),
QoSProfile(depth=1, durability=True))
def target_speed_updated(self, target_speed):
"""
callback on new target speed
"""
self.loginfo("New target speed received: {}".format(target_speed.data))
self.target_speed = target_speed.data
def run_step(self):
"""
Execute one step of navigation.
"""
if not self.agent:
self.loginfo("Waiting for ego vehicle...")
else:
hazard_detected = self.agent.run_step()
speed_command = Float64()
if hazard_detected:
# stop, publish speed 0.0km/h
speed_command.data = 0.0
self.speed_command_publisher.publish(speed_command)
else:
speed_command.data = self.target_speed
self.speed_command_publisher.publish(speed_command)
return
def main():
problem = NumbersState2.random_start()
print problem
agent = BasicAgent(BasicHeuristic2())
# number_of_moves = 5
while problem.get_successors():
solution = agent.solve(problem, 5)
print solution
problem = problem.get_successors()[solution]
print problem
if problem.is_goal():
print 'WIN!!'
return
else:
problem = problem.mutate()
print problem
def main():
problem = NumbersState.random_start()
# problem = NumbersState.from_table(
# [[2, 4, 8, 16],
# [2, 32, 2, 32],
# [8, 16, 4, 64],
# [16, 2, 128, 4]])
print problem
agent = BasicAgent(BasicHeuristic())
# number_of_moves = 5
while not problem.is_goal() and problem.get_successors():
solution = agent.solve(problem, 5)
print solution
problem = problem.get_successors()[solution]
print problem
if not problem.is_goal():
problem = problem.mutate()
print problem
class CarlaAdAgent(object):
"""
A basic AD agent using CARLA waypoints
"""
def __init__(self, role_name, target_speed, avoid_risk):
"""
Constructor
"""
self._route_assigned = False
self._global_plan = None
self._agent = None
self._target_speed = target_speed
rospy.on_shutdown(self.on_shutdown)
# wait for ego vehicle
vehicle_info = None
try:
vehicle_info = rospy.wait_for_message(
"/carla/{}/vehicle_info".format(role_name), CarlaEgoVehicleInfo)
except rospy.ROSException:
rospy.logerr("Timeout while waiting for world info!")
sys.exit(1)
self._route_subscriber = rospy.Subscriber(
"/carla/{}/waypoints".format(role_name), Path, self.path_updated)
self._target_speed_subscriber = rospy.Subscriber(
"/carla/{}/target_speed".format(role_name), Float64, self.target_speed_updated)
self.vehicle_control_publisher = rospy.Publisher(
"/carla/{}/vehicle_control_cmd".format(role_name), CarlaEgoVehicleControl, queue_size=1)
self._agent = BasicAgent(role_name, vehicle_info.id, # pylint: disable=no-member
avoid_risk)
def on_shutdown(self):
"""
callback on shutdown
"""
rospy.loginfo("Shutting down, stopping ego vehicle...")
if self._agent:
self.vehicle_control_publisher.publish(self._agent.emergency_stop())
def target_speed_updated(self, target_speed):
"""
callback on new target speed
"""
rospy.loginfo("New target speed received: {}".format(target_speed.data))
self._target_speed = target_speed.data
def path_updated(self, path):
"""
callback on new route
"""
rospy.loginfo("New plan with {} waypoints received.".format(len(path.poses)))
if self._agent:
self.vehicle_control_publisher.publish(self._agent.emergency_stop())
self._global_plan = path
self._route_assigned = False
def run_step(self):
"""
Execute one step of navigation.
"""
control = CarlaEgoVehicleControl()
control.steer = 0.0
control.throttle = 0.0
control.brake = 0.0
control.hand_brake = False
if not self._agent:
rospy.loginfo("Waiting for ego vehicle...")
return control
if not self._route_assigned and self._global_plan:
rospy.loginfo("Assigning plan...")
self._agent._local_planner.set_global_plan( # pylint: disable=protected-access
self._global_plan.poses)
self._route_assigned = True
else:
control, finished = self._agent.run_step(self._target_speed)
if finished:
self._global_plan = None
self._route_assigned = False
return control
def run(self):
"""
Control loop
:return:
"""
r = rospy.Rate(10)
while not rospy.is_shutdown():
if self._global_plan:
control = self.run_step()
if control:
control.steer = -control.steer
self.vehicle_control_publisher.publish(control)
else:
try:
r.sleep()
except rospy.ROSInterruptException:
pass
mineDensityIndex = 0
mines = int((dimensions ** 2) * density)
mine_densities = [density]
numberOfMines = [mines]
startingCoordinate = (1,1)
trials = 100
trialsConducted = 0
improved_res_v2_5 = {}
improved_res_v2_0 = {}
improved_res_v2_5_sub_trial = []
improved_res_v2_0_sub_trial = []
while(trialsConducted < trials):
mineDensityIndex = len(numberOfMines) - 1
improved_performance_v2_5 = BasicAgent(dimensions, numberOfMines[mineDensityIndex], startingCoordinate, -1, False)
#improved_performance_v2_5.output_agent_map()
i_perf_v2_5 = (numberOfMines[mineDensityIndex] - improved_performance_v2_5.agent_died) / numberOfMines[mineDensityIndex]
improved_res_v2_5_sub_trial.append(i_perf_v2_5)
improved_performance_v2_0 = BasicAgent2(dimensions, numberOfMines[mineDensityIndex], startingCoordinate, improved_performance_v2_5.hidden_map)
#improved_performance_v2_0.output_agent_map()
#print(numberOfMines[mineDensityIndex], " ",improved_performance_v2_0.agent_died)
i_perf_v2_0 = (numberOfMines[mineDensityIndex] - improved_performance_v2_0.agent_died) / numberOfMines[
mineDensityIndex]
improved_res_v2_0_sub_trial.append(i_perf_v2_0)
print("Completed trial #%d" % trialsConducted, " with mine density %.2f" % density)
trialsConducted = trialsConducted + 1
if trialsConducted % 5 == 0:
try:
improved_res_v2_5[density].extend(improved_res_v2_5_sub_trial)
def __init__(self):
BasicAgent.__init__(self, "world")
client = carla.Client('localhost', 2000)
world = client.get_world()
world_map = world.get_map()
dao = GlobalRoutePlannerDAO(world_map)
grp = GlobalRoutePlanner(dao)
grp.setup()
# Obtain route plan
# route = grp.plan_route((43.70042037963867, # -7.828187465667725), (44.610050201416016, -192.88201904296875))
route = grp.plan_route((43.70014190673828, -7.828191757202148),
(44.610050201416016, -192.88201904296875))
print(route)
blueprint = world.get_blueprint_library().find('vehicle.lincoln.mkz2017')
blueprint.set_attribute('role_name', 'hero')
spawn_point = carla.Transform(carla.Location(x=43.7, y=-7.8),
carla.Rotation(yaw=180))
vehicle = world.try_spawn_actor(blueprint, spawn_point)
agent = BasicAgent(vehicle)
agent.set_destination((44.61, -192.88, 0))
while True:
control = agent.run_step()
vehicle.apply_control(control)
time.sleep(0.06)