def __init__(self):
super(LocalPlanner, self).__init__('local_planner')
# ros parameters
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.target_route_point = None
self._vehicle_controller = None
self._waypoints_queue = deque(maxlen=20000)
self._buffer_size = 5
self._waypoint_buffer = deque(maxlen=self._buffer_size)
self.path_received = False
self._current_speed = None
self._current_pose = None
self._target_speed = 0.0
# subscribers
self._odometry_subscriber = self.create_subscriber(
Odometry, "/carla/{}/odometry".format(role_name),
self.odometry_updated)
self._path_subscriber = self.create_subscriber(
Path, "/carla/{}/waypoints".format(role_name), self.path_updated,
QoSProfile(depth=1, durability=True))
self._target_speed_subscriber = self.create_subscriber(
Float64, "/carla/{}/speed_command".format(role_name),
self.target_speed_updated, QoSProfile(depth=1, durability=True))
# publishers
self._target_point_publisher = self.new_publisher(
Marker, "/carla/{}/next_target".format(role_name),
QoSProfile(depth=10, durability=False))
self._control_cmd_publisher = self.new_publisher(
CarlaEgoVehicleControl,
"/carla/{}/vehicle_control_cmd".format(role_name),
QoSProfile(depth=1, durability=False))
# initializing controller
self._vehicle_controller = VehiclePIDController(
self,
args_lateral=args_lateral_dict,
args_longitudinal=args_longitudinal_dict)
# wait for required messages
self.loginfo('Local planner waiting for a path and target speed...')
while self._current_pose is None or self._target_speed is None or self.path_received is False:
time.sleep(0.05)
if ROS_VERSION == 2:
rclpy.spin_once(self, timeout_sec=0)
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=latch_on))
self.traffic_lights_status_publisher = node.new_publisher(
CarlaTrafficLightStatusList,
self.get_topic_prefix() + "/status",
qos_profile=QoSProfile(depth=10, durability=latch_on))
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, 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")
self.vehicle_info_publisher = node.new_publisher(
CarlaEgoVehicleInfo,
self.get_topic_prefix() + "/vehicle_info",
qos_profile=QoSProfile(depth=10, durability=latch_on))
self.control_subscriber = node.create_subscriber(
CarlaEgoVehicleControl,
self.get_topic_prefix() + "/vehicle_control_cmd",
lambda data: self.control_command_updated(data,
manual_override=False))
self.manual_control_subscriber = node.create_subscriber(
CarlaEgoVehicleControl,
self.get_topic_prefix() + "/vehicle_control_cmd_manual",
lambda data: self.control_command_updated(data,
manual_override=True))
self.control_override_subscriber = node.create_subscriber(
Bool,
self.get_topic_prefix() + "/vehicle_control_manual_override",
self.control_command_override, QoSProfile(depth=1,
durability=True))
self.enable_autopilot_subscriber = node.create_subscriber(
Bool,
self.get_topic_prefix() + "/enable_autopilot",
self.enable_autopilot_updated)
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.create_subscriber(
Odometry, "/carla/{}/odometry".format(role_name), self.odometry_cb)
self._path_subscriber = self.create_subscriber(
Path, "/carla/{}/waypoints".format(role_name), self.path_cb,
QoSProfile(depth=1, durability=True))
self._target_speed_subscriber = self.create_subscriber(
Float64, "/carla/{}/speed_command".format(role_name),
self.target_speed_cb, QoSProfile(depth=1, durability=True))
# publishers
self._target_pose_publisher = self.new_publisher(
Marker, "/carla/{}/next_target".format(role_name),
QoSProfile(depth=10, durability=False))
self._control_cmd_publisher = self.new_publisher(
CarlaEgoVehicleControl,
"/carla/{}/vehicle_control_cmd".format(role_name),
QoSProfile(depth=1, durability=False))
# initializing controller
self._vehicle_controller = VehiclePIDController(
self,
args_lateral=args_lateral_dict,
args_longitudinal=args_longitudinal_dict)
def test_vehicle_info(self):
"""
Tests vehicle_info
"""
try:
node = None
ros_init()
node = CompatibleNode('test_node')
msg = node.wait_for_one_message("/carla/ego_vehicle/vehicle_info",
CarlaEgoVehicleInfo,
timeout=TIMEOUT,
qos_profile=QoSProfile(
depth=1, durability=latch_on))
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()
ros_shutdown()
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=1))
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, 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=latch_on))
def __init__(self):
"""
Constructor
"""
super(CarlaWalkerAgent, self).__init__('carla_walker_agent')
role_name = self.get_param("role_name", "ego_vehicle")
self._target_speed = self.get_param("target_speed", 2.0)
self._route_assigned = False
self._waypoints = []
self._current_pose = Pose()
self.on_shutdown(self._on_shutdown)
# wait for ros bridge to create relevant topics
try:
self.wait_for_one_message(
"/carla/{}/odometry".format(role_name), Odometry)
except ROSInterruptException as e:
if not ros_ok:
raise e
self._odometry_subscriber = self.create_subscriber(
Odometry, "/carla/{}/odometry".format(role_name), self.odometry_updated)
self.control_publisher = self.new_publisher(
CarlaWalkerControl, "/carla/{}/walker_control_cmd".format(role_name),
QoSProfile(depth=1, durability=False))
self._route_subscriber = self.create_subscriber(
Path, "/carla/{}/waypoints".format(role_name), self.path_updated)
self._target_speed_subscriber = self.create_subscriber(
Float64, "/carla/{}/target_speed".format(role_name), self.target_speed_updated)
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=latch_on), 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(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=True))
# 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.create_subscriber(
PoseStamped, "/carla/{}/goal".format(self.role_name), self.on_goal)
# 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):
"""
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=True))
self._odometry_subscriber = self.create_subscriber(
Odometry, "/carla/{}/odometry".format(role_name), self.odometry_cb)
self._target_speed_subscriber = self.create_subscriber(
Float64,
"/carla/{}/target_speed".format(role_name),
self.target_speed_cb,
qos_profile=QoSProfile(depth=10, durability=latch_on))
if self._avoid_risk:
self._objects_subscriber = self.create_subscriber(
ObjectArray, "/carla/{}/objects".format(role_name),
self.objects_cb)
self._traffic_light_status_subscriber = self.create_subscriber(
CarlaTrafficLightStatusList,
"/carla/traffic_lights/status",
self.traffic_light_status_cb,
qos_profile=QoSProfile(depth=10, durability=latch_on))
self._traffic_light_info_subscriber = self.create_subscriber(
CarlaTrafficLightInfoList,
"/carla/traffic_lights/info",
self.traffic_light_info_cb,
qos_profile=QoSProfile(depth=10, durability=latch_on))
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
if ROS_VERSION == 1:
self.callback_group = None
elif ROS_VERSION == 2:
self.callback_group = ReentrantCallbackGroup()
fast_qos = QoSProfile(depth=10)
fast_latched_qos = QoSProfile(depth=10, durability=latch_on) # imported from ros_compat.
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, callback_group=self.callback_group)
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, callback_group=self.callback_group)
self.vehicle_control_publisher = \
self.node.new_publisher(CarlaEgoVehicleControl,
"/carla/{}/vehicle_control_cmd_manual".format(self.role_name),
qos_profile=fast_qos, callback_group=self.callback_group)
self.carla_status_subscriber = self.node.create_subscriber(CarlaStatus, "/carla/status",
self._on_new_carla_frame,
callback_group=self.callback_group)
self.set_autopilot(self._autopilot_enabled)
self.set_vehicle_control_manual_override(
self.vehicle_control_manual_override) # disable manual override
def __init__(self, role_name, vehicle_id, avoid_risk, node):
"""
"""
self.node = node
self._proximity_threshold = 10.0 # meters
self._map_name = None
self._vehicle_location = None
self._vehicle_yaw = None
self._vehicle_id = vehicle_id
self._last_traffic_light = None
self._target_route_point = None
self._odometry_subscriber = self.node.create_subscriber(
Odometry, "/carla/{}/odometry".format(role_name),
self.odometry_updated)
# wait for first odometry update
self.node.logwarn('Agent waiting for odometry message')
while self._vehicle_location is None:
time.sleep(0.05)
if ROS_VERSION == 2:
spin_once(node, timeout_sec=0)
self.node.logwarn('Odometry message received')
if avoid_risk:
self._get_waypoint_client = node.create_service_client(
'/carla_waypoint_publisher/{}/get_waypoint'.format(role_name),
GetWaypoint)
self._traffic_lights = []
self._traffic_light_status_subscriber = node.create_subscriber(
CarlaTrafficLightStatusList,
"/carla/traffic_lights/status",
self.traffic_lights_updated,
qos_profile=QoSProfile(depth=10, durability=latch_on))
self._target_point_subscriber = node.create_subscriber(
Marker, "/carla/{}/next_target".format(role_name),
self.target_point_updated)
world_info = node.wait_for_one_message("/carla/world_info",
CarlaWorldInfo,
qos_profile=QoSProfile(
depth=1,
durability=latch_on))
self._map_name = world_info.map_name
def initialize_twist_to_control(self, role_name):
self.create_subscriber(CarlaEgoVehicleInfo,
"/carla/{}/vehicle_info".format(role_name),
self.update_vehicle_info,
qos_profile=QoSProfile(depth=1,
durability=latch_on))
self.create_subscriber(Twist, "/carla/{}/twist".format(role_name),
self.twist_received)
self.pub = self.new_publisher(
CarlaEgoVehicleControl,
"/carla/{}/vehicle_control_cmd".format(role_name))
def test_traffic_lights_info(self):
"""
Tests traffic_lights
"""
try:
node = None
ros_init()
node = CompatibleNode('test_node')
msg = node.wait_for_one_message("/carla/traffic_lights/info",
CarlaTrafficLightInfoList,
timeout=TIMEOUT,
qos_profile=QoSProfile(
depth=10, durability=latch_on))
self.assertNotEqual(len(msg.traffic_lights), 0)
finally:
if node is not None:
node.destroy_node()
ros_shutdown()
def test_map(self):
"""
Tests map
"""
try:
node = None
ros_init()
node = CompatibleNode('test_node')
msg = node.wait_for_one_message("/carla/map",
String,
timeout=TIMEOUT,
qos_profile=QoSProfile(
depth=10, durability=latch_on))
self.assertNotEqual(len(msg.data), 0)
finally:
if node is not None:
node.destroy_node()
ros_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=latch_on))
self.loginfo("Vehicle info received.")
self._ego_vehicle_id = vehicle_info.id
self._get_waypoint_client = self.create_service_client(
'/carla_waypoint_publisher/{}/get_waypoint'.format(role_name),
GetWaypoint)
def test_world_info(self):
"""
Tests world_info
"""
try:
node = None
ros_init()
node = CompatibleNode('test_node')
msg = node.wait_for_one_message("/carla/world_info",
CarlaWorldInfo,
timeout=TIMEOUT,
qos_profile=QoSProfile(
depth=10, durability=latch_on))
self.assertNotEqual(len(msg.map_name), 0)
self.assertNotEqual(len(msg.opendrive), 0)
finally:
if node is not None:
node.destroy_node()
ros_shutdown()
def __init__(self):
"""
Constructor
"""
super(CarlaAckermannControl, self).__init__("carla_ackermann_control",
rospy_init=True)
# PID controller
# the controller has to run with the simulation time, not with real-time
#
# To prevent "float division by zero" within PID controller initialize it with
# a previous point in time (the error happens because the time doesn't
# change between initialization and first call, therefore dt is 0)
sys.modules['simple_pid.PID']._current_time = ( # pylint: disable=protected-access
lambda: self.get_time() - 0.1)
# we might want to use a PID controller to reach the final target speed
self.speed_controller = PID(Kp=self.get_param("speed_Kp",
alternative_value=0.05),
Ki=self.get_param("speed_Ki",
alternative_value=0.),
Kd=self.get_param("speed_Kd",
alternative_value=0.5),
sample_time=0.05,
output_limits=(-1., 1.))
self.accel_controller = PID(Kp=self.get_param("accel_Kp",
alternative_value=0.05),
Ki=self.get_param("accel_Ki",
alternative_value=0.),
Kd=self.get_param("accel_Kd",
alternative_value=0.05),
sample_time=0.05,
output_limits=(-1, 1))
# use the correct time for further calculations
sys.modules['simple_pid.PID']._current_time = ( # pylint: disable=protected-access
lambda: self.get_time())
if ROS_VERSION == 1:
self.reconfigure_server = Server(
EgoVehicleControlParameterConfig,
namespace="",
callback=self.reconfigure_pid_parameters,
)
if ROS_VERSION == 2:
self.set_parameters_callback(self.reconfigure_pid_parameters)
self.control_loop_rate = self.get_param("control_loop_rate", 0.05)
self.lastAckermannMsgReceived = datetime.datetime(
datetime.MINYEAR, 1, 1)
self.vehicle_status = CarlaEgoVehicleStatus()
self.vehicle_info = CarlaEgoVehicleInfo()
self.role_name = self.get_param('role_name', 'ego_vehicle')
# control info
self.info = EgoVehicleControlInfo()
# set initial maximum values
self.vehicle_info_updated(self.vehicle_info)
# target values
self.info.target.steering_angle = 0.
self.info.target.speed = 0.
self.info.target.speed_abs = 0.
self.info.target.accel = 0.
self.info.target.jerk = 0.
# current values
self.info.current.time_sec = self.get_time()
self.info.current.speed = 0.
self.info.current.speed_abs = 0.
self.info.current.accel = 0.
# control values
self.info.status.status = 'n/a'
self.info.status.speed_control_activation_count = 0
self.info.status.speed_control_accel_delta = 0.
self.info.status.speed_control_accel_target = 0.
self.info.status.accel_control_pedal_delta = 0.
self.info.status.accel_control_pedal_target = 0.
self.info.status.brake_upper_border = 0.
self.info.status.throttle_lower_border = 0.
# control output
self.info.output.throttle = 0.
self.info.output.brake = 1.0
self.info.output.steer = 0.
self.info.output.reverse = False
self.info.output.hand_brake = True
# ackermann drive commands
self.control_subscriber = self.create_subscriber(
AckermannDrive, "/carla/" + self.role_name + "/ackermann_cmd",
self.ackermann_command_updated)
# current status of the vehicle
self.vehicle_status_subscriber = self.create_subscriber(
CarlaEgoVehicleStatus,
"/carla/" + self.role_name + "/vehicle_status",
self.vehicle_status_updated,
)
# vehicle info
self.vehicle_info_subscriber = self.create_subscriber(
CarlaEgoVehicleInfo,
"/carla/" + self.role_name + "/vehicle_info",
self.vehicle_info_updated,
)
# to send command to carla
self.carla_control_publisher = self.new_publisher(
CarlaEgoVehicleControl,
"/carla/" + self.role_name + "/vehicle_control_cmd",
qos_profile=QoSProfile(depth=1))
# report controller info
self.control_info_publisher = self.new_publisher(
EgoVehicleControlInfo,
"/carla/" + self.role_name + "/ackermann_control/control_info",
qos_profile=QoSProfile(depth=1))
def __init__(self, actor, args=None):
super(RosVehicleControl, self).__init__(actor)
ros_init(args=None)
self._carla_actor = actor
self._role_name = actor.attributes["role_name"]
if not self._role_name:
if ROS_VERSION == 1:
rospy.logerr("Invalid role_name!")
elif ROS_VERSION == 2:
logging.get_logger("pre_logger").error("Invalid role_name!")
self._path_topic_name = "waypoints"
if "path_topic_name" in args:
self._path_topic_name = args["path_topic_name"]
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=latch_on))
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=latch_on))
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:
cli_args = [launch_package, launch_file]
elif ROS_VERSION == 2:
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))
if ROS_VERSION == 1:
uuid = roslaunch.rlutil.get_or_generate_uuid(None, False)
roslaunch.configure_logging(uuid)
roslaunch_file = roslaunch.rlutil.resolve_launch_arguments(
cli_args)
roslaunch_args = cli_args[2:]
launch_files = [(roslaunch_file[0], roslaunch_args)]
parent = roslaunch.parent.ROSLaunchParent(uuid, launch_files)
parent.start()
elif ROS_VERSION == 2:
cmdline = ['ros2 launch'] + 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))
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()
if ROS_VERSION == 1:
self.tf_listener = tf.TransformListener()
self.callback_group = None
elif ROS_VERSION == 2:
self.tf_buffer = tf2_ros.Buffer()
self.tf_listener = tf2_ros.TransformListener(self.tf_buffer, node=self.node)
self.callback_group = ReentrantCallbackGroup()
self.vehicle_status_subscriber = node.create_subscriber(
CarlaEgoVehicleStatus, "/carla/{}/vehicle_status".format(self.role_name),
self.vehicle_status_updated, callback_group=self.callback_group)
self.vehicle_info = CarlaEgoVehicleInfo()
self.vehicle_info_subscriber = node.create_subscriber(
CarlaEgoVehicleInfo, "/carla/{}/vehicle_info".format(self.role_name),
self.vehicle_info_updated, callback_group=self.callback_group, qos_profile=QoSProfile(depth=10, durability=latch_on))
self.latitude = 0
self.longitude = 0
self.manual_control = False
self.gnss_subscriber = node.create_subscriber(
NavSatFix, "/carla/{}/gnss".format(self.role_name), self.gnss_updated,
callback_group=self.callback_group)
self.manual_control_subscriber = node.create_subscriber(
Bool, "/carla/{}/vehicle_control_manual_override".format(self.role_name),
self.manual_control_override_updated, callback_group=self.callback_group)
self.carla_status = CarlaStatus()
self.status_subscriber = node.create_subscriber(CarlaStatus, "/carla/status",
self.carla_status_updated,
callback_group=self.callback_group)
def __init__(self, context):
"""
Constructor
"""
super(CarlaControlPlugin, self).__init__(context)
self.setObjectName('CARLA Control')
self._widget = QWidget()
self._node = CompatibleNode('rqt_carla_control_node', rospy_init=False)
if ROS_VERSION == 1:
package_share_dir = rospkg.RosPack().get_path('rqt_carla_control')
self.callback_group = None
elif ROS_VERSION == 2:
package_share_dir = get_package_share_directory(
'rqt_carla_control')
self.callback_group = ReentrantCallbackGroup()
ui_file = os.path.join(package_share_dir, 'resource',
'CarlaControl.ui')
loadUi(ui_file, self._widget)
self._widget.setObjectName('CarlaControl')
if context.serial_number() > 1:
self._widget.setWindowTitle(self._widget.windowTitle() +
(' (%d)' % context.serial_number()))
self.pause_icon = QIcon(
QPixmap(os.path.join(package_share_dir, 'resource', 'pause.png')))
self.play_icon = QIcon(
QPixmap(os.path.join(package_share_dir, 'resource', 'play.png')))
self._widget.pushButtonStepOnce.setIcon(
QIcon(
QPixmap(
os.path.join(package_share_dir, 'resource',
'step_once.png'))))
self.carla_status = None
self.carla_status_subscriber = self._node.create_subscriber(
CarlaStatus,
"/carla/status",
self.carla_status_changed,
callback_group=self.callback_group)
self.carla_control_publisher = self._node.new_publisher(
CarlaControl, "/carla/control",
QoSProfile(depth=10, durability=False))
self._widget.pushButtonPlayPause.setDisabled(True)
self._widget.pushButtonStepOnce.setDisabled(True)
self._widget.pushButtonPlayPause.setIcon(self.play_icon)
self._widget.pushButtonPlayPause.clicked.connect(
self.toggle_play_pause)
self._widget.pushButtonStepOnce.clicked.connect(self.step_once)
context.add_widget(self._widget)
if ROS_VERSION == 2:
spin_thread = threading.Thread(target=self._node.spin, daemon=True)
spin_thread.start()