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))
Beispiel #4
0
    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)
Beispiel #5
0
    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)
Beispiel #6
0
 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))
Beispiel #9
0
    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)
Beispiel #12
0
    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
Beispiel #14
0
    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
Beispiel #15
0
    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))
Beispiel #16
0
 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()
Beispiel #17
0
 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()
Beispiel #18
0
    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)
Beispiel #19
0
 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()