def __init__(self, uid, name, parent, node, actor_list):
"""
Constructor
:param uid: unique identifier for this object
:type uid: int
:param name: name identiying the sensor
:type name: string
:param parent: the parent of this
:type parent: carla_ros_bridge.Parent
:param node: node-handle
:type node: CompatibleNode
:param actor_list: current list of actors
:type actor_list: map(carla-actor-id -> python-actor-object)
"""
super(TrafficLightsSensor, self).__init__(uid=uid,
name=name,
parent=parent,
node=node)
self.actor_list = actor_list
self.traffic_light_status = CarlaTrafficLightStatusList()
self.traffic_light_actors = []
self.traffic_lights_info_publisher = node.new_publisher(
CarlaTrafficLightInfoList,
self.get_topic_prefix() + "/info",
qos_profile=QoSProfile(
depth=10, durability=DurabilityPolicy.TRANSIENT_LOCAL))
self.traffic_lights_status_publisher = node.new_publisher(
CarlaTrafficLightStatusList,
self.get_topic_prefix() + "/status",
qos_profile=QoSProfile(
depth=10, durability=DurabilityPolicy.TRANSIENT_LOCAL))
def __init__(self, uid, name, parent, node, carla_actor, vehicle_control_applied_callback):
"""
Constructor
:param uid: unique identifier for this object
:type uid: int
:param name: name identiying this object
:type name: string
:param parent: the parent of this
:type parent: carla_ros_bridge.Parent
:param node: node-handle
:type node: CompatibleNode
:param carla_actor: carla actor object
:type carla_actor: carla.Actor
"""
super(EgoVehicle, self).__init__(uid=uid,
name=name,
parent=parent,
node=node,
carla_actor=carla_actor)
self.vehicle_info_published = False
self.vehicle_control_override = False
self._vehicle_control_applied_callback = vehicle_control_applied_callback
self.vehicle_status_publisher = node.new_publisher(
CarlaEgoVehicleStatus,
self.get_topic_prefix() + "/vehicle_status",
qos_profile=10)
self.vehicle_info_publisher = node.new_publisher(
CarlaEgoVehicleInfo,
self.get_topic_prefix() +
"/vehicle_info",
qos_profile=QoSProfile(depth=10, durability=DurabilityPolicy.TRANSIENT_LOCAL))
self.control_subscriber = node.new_subscription(
CarlaEgoVehicleControl,
self.get_topic_prefix() + "/vehicle_control_cmd",
lambda data: self.control_command_updated(data, manual_override=False),
qos_profile=10)
self.manual_control_subscriber = node.new_subscription(
CarlaEgoVehicleControl,
self.get_topic_prefix() + "/vehicle_control_cmd_manual",
lambda data: self.control_command_updated(data, manual_override=True),
qos_profile=10)
self.control_override_subscriber = node.new_subscription(
Bool,
self.get_topic_prefix() + "/vehicle_control_manual_override",
self.control_command_override,
qos_profile=QoSProfile(depth=1, durability=DurabilityPolicy.TRANSIENT_LOCAL))
self.enable_autopilot_subscriber = node.new_subscription(
Bool,
self.get_topic_prefix() + "/enable_autopilot",
self.enable_autopilot_updated,
qos_profile=10)
def __init__(self):
super(LocalPlanner, self).__init__("local_planner")
role_name = self.get_param("role_name", "ego_vehicle")
self.control_time_step = self.get_param("control_time_step", 0.05)
args_lateral_dict = {}
args_lateral_dict['K_P'] = self.get_param("Kp_lateral", 0.9)
args_lateral_dict['K_I'] = self.get_param("Ki_lateral", 0.0)
args_lateral_dict['K_D'] = self.get_param("Kd_lateral", 0.0)
args_longitudinal_dict = {}
args_longitudinal_dict['K_P'] = self.get_param("Kp_longitudinal",
0.206)
args_longitudinal_dict['K_I'] = self.get_param("Ki_longitudinal",
0.0206)
args_longitudinal_dict['K_D'] = self.get_param("Kd_longitudinal",
0.515)
self.data_lock = threading.Lock()
self._current_pose = None
self._current_speed = None
self._target_speed = 0.0
self._buffer_size = 5
self._waypoints_queue = collections.deque(maxlen=20000)
self._waypoint_buffer = collections.deque(maxlen=self._buffer_size)
# subscribers
self._odometry_subscriber = self.new_subscription(
Odometry,
"/carla/{}/odometry".format(role_name),
self.odometry_cb,
qos_profile=10)
self._path_subscriber = self.new_subscription(
Path, "/carla/{}/waypoints".format(role_name), self.path_cb,
QoSProfile(depth=1, durability=DurabilityPolicy.TRANSIENT_LOCAL))
self._target_speed_subscriber = self.new_subscription(
Float64, "/carla/{}/speed_command".format(role_name),
self.target_speed_cb,
QoSProfile(depth=1, durability=DurabilityPolicy.TRANSIENT_LOCAL))
# publishers
self._target_pose_publisher = self.new_publisher(
Marker, "/carla/{}/next_target".format(role_name), qos_profile=10)
self._control_cmd_publisher = self.new_publisher(
CarlaEgoVehicleControl,
"/carla/{}/vehicle_control_cmd".format(role_name),
qos_profile=10)
# initializing controller
self._vehicle_controller = VehiclePIDController(
self,
args_lateral=args_lateral_dict,
args_longitudinal=args_longitudinal_dict)
def __init__(self):
"""
Constructor
"""
super(CarlaAdAgent, self).__init__("ad_agent")
role_name = self.get_param("role_name", "ego_vehicle")
self._avoid_risk = self.get_param("avoid_risk", True)
self.data_lock = threading.Lock()
self._ego_vehicle_pose = None
self._objects = {}
self._lights_status = {}
self._lights_info = {}
self._target_speed = 0.
self.speed_command_publisher = self.new_publisher(
Float64, "/carla/{}/speed_command".format(role_name),
QoSProfile(depth=1, durability=DurabilityPolicy.TRANSIENT_LOCAL))
self._odometry_subscriber = self.new_subscription(
Odometry,
"/carla/{}/odometry".format(role_name),
self.odometry_cb,
qos_profile=10
)
self._target_speed_subscriber = self.new_subscription(
Float64,
"/carla/{}/target_speed".format(role_name),
self.target_speed_cb,
qos_profile=QoSProfile(depth=10, durability=DurabilityPolicy.TRANSIENT_LOCAL)
)
if self._avoid_risk:
self._objects_subscriber = self.new_subscription(
ObjectArray,
"/carla/{}/objects".format(role_name),
self.objects_cb,
qos_profile=10
)
self._traffic_light_status_subscriber = self.new_subscription(
CarlaTrafficLightStatusList,
"/carla/traffic_lights/status",
self.traffic_light_status_cb,
qos_profile=QoSProfile(depth=10, durability=DurabilityPolicy.TRANSIENT_LOCAL)
)
self._traffic_light_info_subscriber = self.new_subscription(
CarlaTrafficLightInfoList,
"/carla/traffic_lights/info",
self.traffic_light_info_cb,
qos_profile=QoSProfile(depth=10, durability=DurabilityPolicy.TRANSIENT_LOCAL)
)
def test_vehicle_info(self):
"""
Tests vehicle_info
"""
try:
node = None
roscomp.init("test_node")
node = CompatibleNode('test_node')
msg = node.wait_for_message(
"/carla/ego_vehicle/vehicle_info",
CarlaEgoVehicleInfo,
timeout=TIMEOUT,
qos_profile=QoSProfile(
depth=1, durability=DurabilityPolicy.TRANSIENT_LOCAL))
self.assertNotEqual(msg.id, 0)
self.assertEqual(msg.type, "vehicle.tesla.model3")
self.assertEqual(msg.rolename, "ego_vehicle")
self.assertEqual(len(msg.wheels), 4)
self.assertNotEqual(msg.max_rpm, 0.0)
self.assertNotEqual(msg.moi, 0.0)
self.assertNotEqual(msg.damping_rate_full_throttle, 0.0)
self.assertNotEqual(msg.damping_rate_zero_throttle_clutch_engaged,
0.0)
self.assertNotEqual(
msg.damping_rate_zero_throttle_clutch_disengaged, 0.0)
self.assertTrue(msg.use_gear_autobox)
self.assertNotEqual(msg.gear_switch_time, 0.0)
self.assertNotEqual(msg.mass, 0.0)
self.assertNotEqual(msg.clutch_strength, 0.0)
self.assertNotEqual(msg.drag_coefficient, 0.0)
self.assertNotEqual(msg.center_of_mass, Vector3())
finally:
if node is not None:
node.destroy_node()
roscomp.shutdown()
def __init__(self, role_name, width, height, node):
self.role_name = role_name
self.dim = (width, height)
self.node = node
font = pygame.font.Font(pygame.font.get_default_font(), 20)
fonts = [x for x in pygame.font.get_fonts() if 'mono' in x]
default_font = 'ubuntumono'
mono = default_font if default_font in fonts else fonts[0]
mono = pygame.font.match_font(mono)
self._font_mono = pygame.font.Font(mono, 14)
self._notifications = FadingText(font, (width, 40), (0, height - 40))
self.help = HelpText(pygame.font.Font(mono, 24), width, height)
self._show_info = True
self._info_text = []
self.vehicle_status = CarlaEgoVehicleStatus()
self.vehicle_status_subscriber = node.new_subscription(
CarlaEgoVehicleStatus,
"/carla/{}/vehicle_status".format(self.role_name),
self.vehicle_status_updated,
qos_profile=10)
self.vehicle_info = CarlaEgoVehicleInfo()
self.vehicle_info_subscriber = node.new_subscription(
CarlaEgoVehicleInfo,
"/carla/{}/vehicle_info".format(self.role_name),
self.vehicle_info_updated,
qos_profile=QoSProfile(
depth=10, durability=DurabilityPolicy.TRANSIENT_LOCAL))
self.x, self.y, self.z = 0, 0, 0
self.yaw = 0
self.latitude = 0
self.longitude = 0
self.manual_control = False
self.gnss_subscriber = node.new_subscription(NavSatFix,
"/carla/{}/gnss".format(
self.role_name),
self.gnss_updated,
qos_profile=10)
self.odometry_subscriber = node.new_subscription(
Odometry,
"/carla/{}/odometry".format(self.role_name),
self.odometry_updated,
qos_profile=10)
self.manual_control_subscriber = node.new_subscription(
Bool,
"/carla/{}/vehicle_control_manual_override".format(self.role_name),
self.manual_control_override_updated,
qos_profile=10)
self.carla_status = CarlaStatus()
self.status_subscriber = node.new_subscription(
CarlaStatus,
"/carla/status",
self.carla_status_updated,
qos_profile=10)
def connect_to_carla(self):
self.loginfo("Waiting for CARLA world (topic: /carla/world_info)...")
try:
self.wait_for_message(
"/carla/world_info",
CarlaWorldInfo,
qos_profile=QoSProfile(
depth=1, durability=DurabilityPolicy.TRANSIENT_LOCAL),
timeout=15.0)
except ROSException as e:
self.logerr("Error while waiting for world info: {}".format(e))
raise e
host = self.get_param("host", "127.0.0.1")
port = self.get_param("port", 2000)
timeout = self.get_param("timeout", 10)
self.loginfo(
"CARLA world available. Trying to connect to {host}:{port}".format(
host=host, port=port))
carla_client = carla.Client(host=host, port=port)
carla_client.set_timeout(timeout)
try:
self.world = carla_client.get_world()
except RuntimeError as e:
self.logerr("Error while connecting to Carla: {}".format(e))
raise e
self.loginfo("Connected to Carla.")
def __init__(self):
"""
Constructor
"""
super(CarlaRosScenarioRunner, self).__init__('carla_ros_scenario_runner')
role_name = self.get_param("role_name", "ego_vehicle")
scenario_runner_path = self.get_param("scenario_runner_path", "")
wait_for_ego = self.get_param("wait_for_ego", "True")
host = self.get_param("host", "localhost")
port = self.get_param("port", 2000)
self._status_publisher = self.new_publisher(
CarlaScenarioRunnerStatus,
"/scenario_runner/status",
qos_profile=QoSProfile(depth=1, durability=DurabilityPolicy.TRANSIENT_LOCAL))
self.scenario_runner_status_updated(ApplicationStatus.STOPPED)
self._scenario_runner = ScenarioRunnerRunner(
scenario_runner_path,
host,
port,
wait_for_ego,
self.scenario_runner_status_updated,
self.scenario_runner_log)
self._request_queue = queue.Queue()
self._execute_scenario_service = self.new_service(
ExecuteScenario,
'/scenario_runner/execute_scenario',
self.execute_scenario)
def __init__(self):
"""
Constructor
"""
super(TwistToVehicleControl, self).__init__("twist_to_control")
self.role_name = self.get_param("role_name", "ego_vehicle")
self.max_steering_angle = None
self.new_subscription(
CarlaEgoVehicleInfo,
"/carla/{}/vehicle_info".format(self.role_name),
self.update_vehicle_info,
qos_profile=QoSProfile(depth=1, durability=DurabilityPolicy.TRANSIENT_LOCAL))
self.new_subscription(
Twist,
"/carla/{}/twist".format(self.role_name),
self.twist_received,
qos_profile=10)
self.pub = self.new_publisher(
CarlaEgoVehicleControl,
"/carla/{}/vehicle_control_cmd".format(self.role_name),
qos_profile=10)
def __init__(self, uid, name, parent, node, carla_map):
"""
Constructor
:param uid: unique identifier for this object
:type uid: int
:param name: name identiying this object
:type name: string
:param parent: the parent of this
:type parent: carla_ros_bridge.Parent
:param node: node-handle
:type node: carla_ros_bridge.CarlaRosBridge
:param carla_map: carla map object
:type carla_map: carla.Map
"""
super(OpenDriveSensor, self).__init__(uid=uid,
name=name,
parent=parent,
node=node)
self.carla_map = carla_map
self._map_published = False
self.map_publisher = node.new_publisher(
String,
self.get_topic_prefix(),
qos_profile=QoSProfile(
depth=10, durability=DurabilityPolicy.TRANSIENT_LOCAL))
def __init__(self, role_name, hud, node):
self.role_name = role_name
self.hud = hud
self.node = node
self._autopilot_enabled = False
self._control = CarlaEgoVehicleControl()
self._steer_cache = 0.0
fast_qos = QoSProfile(depth=10)
fast_latched_qos = QoSProfile(
depth=10, durability=DurabilityPolicy.TRANSIENT_LOCAL)
self.vehicle_control_manual_override_publisher = self.node.new_publisher(
Bool,
"/carla/{}/vehicle_control_manual_override".format(self.role_name),
qos_profile=fast_latched_qos)
self.vehicle_control_manual_override = False
self.auto_pilot_enable_publisher = self.node.new_publisher(
Bool,
"/carla/{}/enable_autopilot".format(self.role_name),
qos_profile=fast_qos)
self.vehicle_control_publisher = self.node.new_publisher(
CarlaEgoVehicleControl,
"/carla/{}/vehicle_control_cmd_manual".format(self.role_name),
qos_profile=fast_qos)
self.carla_status_subscriber = self.node.new_subscription(
CarlaStatus,
"/carla/status",
self._on_new_carla_frame,
qos_profile=10)
self.set_autopilot(self._autopilot_enabled)
self.set_vehicle_control_manual_override(
self.vehicle_control_manual_override) # disable manual override
def __init__(self, carla_world, node):
"""
Constructor
:param carla_world: carla world object
:type carla_world: carla.World
:param node: node-handle
:type node: CompatibleNode
"""
self.node = node
self.carla_map = carla_world.get_map()
self.map_published = False
self.world_info_publisher = node.new_publisher(
CarlaWorldInfo,
"/carla/world_info",
qos_profile=QoSProfile(
depth=10, durability=DurabilityPolicy.TRANSIENT_LOCAL))
def test_traffic_lights_info(self):
"""
Tests traffic_lights
"""
try:
node = None
roscomp.init("test_node")
node = CompatibleNode('test_node')
msg = node.wait_for_message(
"/carla/traffic_lights/info",
CarlaTrafficLightInfoList,
timeout=TIMEOUT,
qos_profile=QoSProfile(
depth=10, durability=DurabilityPolicy.TRANSIENT_LOCAL))
self.assertNotEqual(len(msg.traffic_lights), 0)
finally:
if node is not None:
node.destroy_node()
roscomp.shutdown()
def test_map(self):
"""
Tests map
"""
try:
node = None
roscomp.init("test_node")
node = CompatibleNode('test_node')
msg = node.wait_for_message(
"/carla/map",
String,
timeout=TIMEOUT,
qos_profile=QoSProfile(
depth=10, durability=DurabilityPolicy.TRANSIENT_LOCAL))
self.assertNotEqual(len(msg.data), 0)
finally:
if node is not None:
node.destroy_node()
roscomp.shutdown()
def __init__(self, name="agent"):
super(Agent, self).__init__(name)
self._proximity_tlight_threshold = 10.0 # meters
self._proximity_vehicle_threshold = 12.0 # meters
role_name = self.get_param("role_name", "ego_vehicle")
self.loginfo("Waiting for vehicle_info...")
vehicle_info = self.wait_for_message(
"/carla/{}/vehicle_info".format(role_name),
CarlaEgoVehicleInfo,
qos_profile=QoSProfile(
depth=10, durability=DurabilityPolicy.TRANSIENT_LOCAL))
self.loginfo("Vehicle info received.")
self._ego_vehicle_id = vehicle_info.id
self._get_waypoint_client = self.new_client(
GetWaypoint,
'/carla_waypoint_publisher/{}/get_waypoint'.format(role_name),
callback_group=MutuallyExclusiveCallbackGroup())
def test_world_info(self):
"""
Tests world_info
"""
try:
node = None
roscomp.init("test_node")
node = CompatibleNode('test_node')
msg = node.wait_for_message(
"/carla/world_info",
CarlaWorldInfo,
timeout=TIMEOUT,
qos_profile=QoSProfile(
depth=10, durability=DurabilityPolicy.TRANSIENT_LOCAL))
self.assertNotEqual(len(msg.map_name), 0)
self.assertNotEqual(len(msg.opendrive), 0)
finally:
if node is not None:
node.destroy_node()
roscomp.shutdown()
def __init__(self, uid, name, parent, node, actor_list, world):
"""
Constructor
:param uid: unique identifier for this object
:type uid: int
:param name: name identiying this object
:type name: string
:param carla_world: carla world object
:type carla_world: carla.World
:param parent: the parent of this
:type parent: carla_ros_bridge.Parent
:param node: node-handle
:type node: carla_ros_bridge.CarlaRosBridge
:param actor_list: current list of actors
:type actor_list: map(carla-actor-id -> python-actor-object)
"""
super(MarkerSensor, self).__init__(uid=uid,
name=name,
parent=parent,
node=node)
self.actor_list = actor_list
self.world = world
self.node = node
self.marker_publisher = node.new_publisher(MarkerArray,
self.get_topic_prefix(),
qos_profile=10)
self.static_marker_publisher = node.new_publisher(
MarkerArray,
self.get_topic_prefix() + "/static",
qos_profile=QoSProfile(
depth=1, durability=DurabilityPolicy.TRANSIENT_LOCAL))
# id generator for static objects.
self.static_id_gen = itertools.count(1)
static_markers = self._get_static_markers(OBJECT_LABELS.keys())
if static_markers:
self.static_marker_publisher.publish(static_markers)
def __init__(self):
"""
Constructor
"""
super(CarlaToRosWaypointConverter,
self).__init__('carla_waypoint_publisher')
self.connect_to_carla()
self.map = self.world.get_map()
self.ego_vehicle = None
self.ego_vehicle_location = None
self.on_tick = None
self.role_name = self.get_param("role_name", 'ego_vehicle')
self.waypoint_publisher = self.new_publisher(
Path, '/carla/{}/waypoints'.format(self.role_name),
QoSProfile(depth=1, durability=DurabilityPolicy.TRANSIENT_LOCAL))
# initialize ros services
self.get_waypoint_service = self.new_service(
GetWaypoint,
'/carla_waypoint_publisher/{}/get_waypoint'.format(self.role_name),
self.get_waypoint)
self.get_actor_waypoint_service = self.new_service(
GetActorWaypoint,
'/carla_waypoint_publisher/{}/get_actor_waypoint'.format(
self.role_name), self.get_actor_waypoint)
# set initial goal
self.goal = self.world.get_map().get_spawn_points()[0]
self.current_route = None
self.goal_subscriber = self.new_subscription(PoseStamped,
"/carla/{}/goal".format(
self.role_name),
self.on_goal,
qos_profile=10)
# use callback to wait for ego vehicle
self.loginfo("Waiting for ego vehicle...")
self.on_tick = self.world.on_tick(self.find_ego_vehicle_actor)
def __init__(self, actor, args=None):
super(RosVehicleControl, self).__init__(actor)
self._carla_actor = actor
self._role_name = actor.attributes["role_name"]
if not self._role_name:
roscomp.logerr("Invalid role_name")
self._path_topic_name = "waypoints"
if "path_topic_name" in args:
self._path_topic_name = args["path_topic_name"]
roscomp.init("ros_agent_{}".format(self._role_name), args=None)
self.node = CompatibleNode('ros_agent_{}'.format(self._role_name))
self._current_target_speed = None
self._current_path = None
self.controller_launch = None
self._target_speed_publisher = self.node.new_publisher(
Float64, "/carla/{}/target_speed".format(self._role_name),
QoSProfile(depth=10, durability=DurabilityPolicy.TRANSIENT_LOCAL))
self.node.loginfo(
"Publishing target_speed on /carla/{}/target_speed".format(
self._role_name))
self._path_publisher = self.node.new_publisher(
Path, "/carla/{}/{}".format(self._role_name,
self._path_topic_name),
QoSProfile(depth=10, durability=DurabilityPolicy.TRANSIENT_LOCAL))
self.node.loginfo("Publishing path on /carla/{}/{}".format(
self._role_name, self._path_topic_name))
if "launch" in args and "launch-package" in args:
launch_file = args["launch"]
launch_package = args["launch-package"]
if ROS_VERSION == 1:
executable = "roslaunch"
cli_args = [launch_package, launch_file]
elif ROS_VERSION == 2:
executable = "ros2 launch"
cli_args = [launch_package, launch_file + '.py']
cli_args.append('role_name:={}'.format(self._role_name))
# add additional launch parameters
launch_parameters = []
for key, value in args.items():
if not key == "launch" and not key == "launch-package" and not key == "path_topic_name":
launch_parameters.append('{}:={}'.format(key, value))
cli_args.append('{}:={}'.format(key, value))
self.node.loginfo(
"{}: Launching {} from package {} (parameters: {})...".format(
self._role_name, launch_file, launch_package,
launch_parameters))
cmdline = [executable] + cli_args
self.controller_launch = ApplicationRunner(
self.controller_runner_status_updated,
self.controller_runner_log,
'RosVehicleControl: launching controller node')
self.controller_launch.execute(
cmdline,
env=os.environ,
)
self.node.loginfo(
"{}: Successfully started ros vehicle control".format(
self._role_name))
else:
self.node.logwarn(
"{}: Missing value for 'launch' and/or 'launch-package'.".
format(self._role_name))