def main(args=None): """ main function :return: """ ros_init(args) controller = None try: controller = CarlaAdAgent() while ros_ok(): time.sleep(0.01) if ROS_VERSION == 2: rclpy.spin_once(controller) controller.run_step() except (ROSInterruptException, ROSException) as e: if ros_ok(): logwarn("ROS Error during exection: {}".format(e)) except KeyboardInterrupt: loginfo("User requested shut down.") finally: if controller is not None: stopping_speed = Float64() stopping_speed.data = 0.0 controller.speed_command_publisher.publish(stopping_speed) ros_shutdown()
def get_waypoint(self, location): """ Helper to get waypoint for location via ros service. Only used if risk should be avoided. """ if not ros_ok(): return None try: response = self.node.call_service(self._get_waypoint_client, location) return response.waypoint except ServiceException as e: if ros_ok(): self.node.logwarn( "Service call 'get_waypoint' failed: {}".format(str(e)))
def run(self): """ Control loop :return: """ current_req = None while ros_ok(): if current_req: if self._scenario_runner.is_running(): self.loginfo( "Scenario Runner currently running. Shutting down.") self._scenario_runner.shutdown() self.loginfo("Scenario Runner stopped.") self.loginfo("Executing scenario {}...".format( current_req.name)) # execute scenario scenario_executed = self._scenario_runner.execute_scenario( current_req.scenario_file) if not scenario_executed: self.logwarn("Unable to execute scenario.") current_req = None else: try: current_req = self._request_queue.get(block=True, timeout=0.5) except queue.Empty: # no new request pass if self._scenario_runner.is_running(): self.loginfo("Scenario Runner currently running. Shutting down.") self._scenario_runner.shutdown()
def main(args=None): """ main function :return: """ ros_init(args) controller = None try: executor = MultiThreadedExecutor() controller = CarlaAdAgent() executor.add_node(controller) update_timer = controller.new_timer( 0.05, lambda timer_event=None: controller.run_step()) controller.spin() except (ROSInterruptException, ROSException) as e: if ros_ok(): logwarn("ROS Error during exection: {}".format(e)) except KeyboardInterrupt: loginfo("User requested shut down.") finally: if controller is not None: stopping_speed = Float64() stopping_speed.data = 0.0 controller.speed_command_publisher.publish(stopping_speed) ros_shutdown()
def _callback_sensor_data(self, carla_sensor_data): """ Callback function called whenever new sensor data is received :param carla_sensor_data: carla sensor data object :type carla_sensor_data: carla.SensorData """ if not self._callback_active.acquire(False): # if acquire fails, sensor is currently getting destroyed return if self.synchronous_mode: if self.sensor_tick_time: self.next_data_expected_time = carla_sensor_data.timestamp + \ float(self.sensor_tick_time) self.queue.put(carla_sensor_data) else: self.publish_tf( trans.carla_transform_to_ros_pose(carla_sensor_data.transform), carla_sensor_data.timestamp) try: self.sensor_data_updated(carla_sensor_data) except ROSException: if ros_ok(): self.node.logwarn( "Sensor {}: Error while executing sensor_data_updated()." .format(self.uid)) self._callback_active.release()
def _update_synchronous_sensor(self, frame, timestamp): while not self.next_data_expected_time or \ (not self.queue.empty() or self.next_data_expected_time and self.next_data_expected_time < timestamp): while True: try: carla_sensor_data = self.queue.get(timeout=1.0) if carla_sensor_data.frame == frame: self.node.logdebug("{}({}): process {}".format( self.__class__.__name__, self.get_id(), frame)) self.publish_tf( trans.carla_transform_to_ros_pose( carla_sensor_data.transform), timestamp) self.sensor_data_updated(carla_sensor_data) return elif carla_sensor_data.frame < frame: self.node.logwarn( "{}({}): skipping old frame {}, expected {}". format(self.__class__.__name__, self.get_id(), carla_sensor_data.frame, frame)) except queue.Empty: if ros_ok(): self.node.logwarn( "{}({}): Expected Frame {} not received".format( self.__class__.__name__, self.get_id(), frame)) return
def process_run_state(self): """ process state changes """ command = None # get last command while not self.carla_control_queue.empty(): command = self.carla_control_queue.get() while command is not None and ros_ok(): self.carla_run_state = command if self.carla_run_state == CarlaControl.PAUSE: # wait for next command self.loginfo("State set to PAUSED") self.status_publisher.set_synchronous_mode_running(False) command = self.carla_control_queue.get() elif self.carla_run_state == CarlaControl.PLAY: self.loginfo("State set to PLAY") self.status_publisher.set_synchronous_mode_running(True) return elif self.carla_run_state == CarlaControl.STEP_ONCE: self.loginfo("Execute single step.") self.status_publisher.set_synchronous_mode_running(True) self.carla_control_queue.put(CarlaControl.PAUSE) return
def publish_tf(self, pose, timestamp): transform = self.get_ros_transform(pose, timestamp) try: self._tf_broadcaster.sendTransform(transform) except ROSException: if ros_ok(): self.node.logwarn("Sensor {} failed to send transform.".format( self.uid))
def get_waypoint(self, location): """ Helper method to get a waypoint for a location. :param location: location request :type location: geometry_msgs/Point :return: waypoint of the requested location :rtype: carla_msgs/Waypoint """ if not ros_ok(): return None try: request = get_service_request(GetWaypoint) request.location = location response = self.call_service(self._get_waypoint_client, request) return response.waypoint except ServiceException as e: if ros_ok(): self.logwarn("Service call 'get_waypoint' failed: {}".format( str(e)))
def update_clock(self, carla_timestamp): """ perform the update of the clock :param carla_timestamp: the current carla time :type carla_timestamp: carla.Timestamp :return: """ if ros_ok(): self.ros_timestamp = ros_timestamp(carla_timestamp.elapsed_seconds, from_sec=True) self.clock_publisher.publish(Clock(clock=self.ros_timestamp))
def get_actor_waypoint(self, actor_id): """ helper method to get waypoint for actor via ros service Only used if risk should be avoided. """ try: request = get_service_request(GetActorWaypoint) request.id = actor_id response = self.node.call_service(self._get_actor_waypoint_client, request) return response.waypoint except ServiceException as e: if ros_ok(): self.node.logwarn("Service call failed: {}".format(e))
def run(self): """ Control loop :return: """ loop_frequency = 20 if ROS_VERSION == 1: r = rospy.Rate(loop_frequency) self.loginfo("Starting run loop") while ros_ok(): if self._waypoints: control = CarlaWalkerControl() direction = Vector3() direction.x = self._waypoints[ 0].position.x - self._current_pose.position.x direction.y = self._waypoints[ 0].position.y - self._current_pose.position.y direction_norm = math.sqrt(direction.x**2 + direction.y**2) if direction_norm > CarlaWalkerAgent.MIN_DISTANCE: control.speed = self._target_speed control.direction.x = direction.x / direction_norm control.direction.y = direction.y / direction_norm else: self._waypoints = self._waypoints[1:] if self._waypoints: self.loginfo("next waypoint: {} {}".format( self._waypoints[0].position.x, self._waypoints[0].position.y)) else: self.loginfo("Route finished.") self.control_publisher.publish(control) try: if ROS_VERSION == 1: r.sleep() elif ROS_VERSION == 2: # TODO: use rclpy.Rate, not working yet time.sleep(1 / loop_frequency) except ROSInterruptException: pass
def _is_light_red_europe_style(self, lights_list): """ This method is specialized to check European style traffic lights. :param lights_list: list containing TrafficLight objects :return: a tuple given by (bool_flag, traffic_light), where - bool_flag is True if there is a traffic light in RED affecting us and False otherwise - traffic_light is the object itself or None if there is no red traffic light affecting us """ if self._vehicle_location is None: # no available self location yet return (False, None) ego_vehicle_location = get_service_request(GetWaypoint) ego_vehicle_location.location = self._vehicle_location ego_vehicle_waypoint = self.get_waypoint(ego_vehicle_location) if not ego_vehicle_waypoint: if ros_ok(): self.node.logwarn("Could not get waypoint for ego vehicle.") return (False, None) for traffic_light in lights_list: object_waypoint = traffic_light[1] if object_waypoint.road_id != ego_vehicle_waypoint.road_id or \ object_waypoint.lane_id != ego_vehicle_waypoint.lane_id: continue if is_within_distance_ahead(object_waypoint.pose.position, ego_vehicle_location.location, math.degrees(self._vehicle_yaw), self._proximity_threshold): traffic_light_state = CarlaTrafficLightStatus.RED for status in self._traffic_lights: if status.id == traffic_light[0]: traffic_light_state = status.state break if traffic_light_state == CarlaTrafficLightStatus.RED: return (True, traffic_light[0]) return (False, None)
def _synchronous_mode_update(self): """ execution loop for synchronous mode """ while not self.shutdown.is_set() and ros_ok(): self.process_run_state() if self.parameters[ 'synchronous_mode_wait_for_vehicle_control_command']: # fill list of available ego vehicles self._expected_ego_vehicle_control_command_ids = [] with self._expected_ego_vehicle_control_command_ids_lock: for actor_id, actor in self.actor_factory.actors.items(): if isinstance(actor, EgoVehicle): self._expected_ego_vehicle_control_command_ids.append( actor_id) frame = self.carla_world.tick() world_snapshot = self.carla_world.get_snapshot() self.status_publisher.set_frame(frame) self.update_clock(world_snapshot.timestamp) self.logdebug( "Tick for frame {} returned. Waiting for sensor data...". format(frame)) self._update(frame, world_snapshot.timestamp.elapsed_seconds) self.logdebug("Waiting for sensor data finished.") self.actor_factory.update_available_objects() if self.parameters[ 'synchronous_mode_wait_for_vehicle_control_command']: # wait for all ego vehicles to send a vehicle control command if self._expected_ego_vehicle_control_command_ids: if not self._all_vehicle_control_commands_received.wait( CarlaRosBridge.VEHICLE_CONTROL_TIMEOUT): self.logwarn( "Timeout ({}s) while waiting for vehicle control commands. " "Missing command from actor ids {}".format( CarlaRosBridge.VEHICLE_CONTROL_TIMEOUT, self. _expected_ego_vehicle_control_command_ids)) self._all_vehicle_control_commands_received.clear()
def main(args=None): """ main function """ ros_init(args) # resolution should be similar to spawned camera with role-name 'view' resolution = {"width": 800, "height": 600} pygame.init() pygame.font.init() pygame.display.set_caption("CARLA ROS manual control") try: display = pygame.display.set_mode((resolution['width'], resolution['height']), pygame.HWSURFACE | pygame.DOUBLEBUF) manual_control_node = ManualControl(resolution) clock = pygame.time.Clock() if ROS_VERSION == 2: executer = rclpy.executors.MultiThreadedExecutor() executer.add_node(manual_control_node) spin_thread = Thread(target=executer.spin) spin_thread.start() while ros_ok(): clock.tick_busy_loop(60) if manual_control_node.render(clock, display): return pygame.display.flip() except KeyboardInterrupt: loginfo("User requested shut down.") finally: ros_shutdown() if ROS_VERSION == 2: spin_thread.join() pygame.quit()
def publish_tf(self, pose, timestamp): if self.synchronous_mode: if not self.relative_spawn_pose: self.node.logwarn("{}: No relative spawn pose defined".format( self.get_prefix())) return pose = self.relative_spawn_pose child_frame_id = self.get_prefix() if self.parent is not None: frame_id = self.parent.get_prefix() else: frame_id = "map" else: child_frame_id = self.get_prefix() frame_id = "map" transform = tf2_ros.TransformStamped() transform.header.stamp = ros_timestamp(sec=timestamp, from_sec=True) transform.header.frame_id = frame_id transform.child_frame_id = child_frame_id transform.transform.translation.x = pose.position.x transform.transform.translation.y = pose.position.y transform.transform.translation.z = pose.position.z transform.transform.rotation.x = pose.orientation.x transform.transform.rotation.y = pose.orientation.y transform.transform.rotation.z = pose.orientation.z transform.transform.rotation.w = pose.orientation.w try: self._tf_broadcaster.sendTransform(transform) except ROSException: if ros_ok(): self.node.logwarn("Sensor {} failed to send transform.".format( self.uid))
def twist_received(self, twist): """ receive twist and convert to carla vehicle control """ if self.max_steering_angle is None: self.logwarn("Did not yet receive vehicle info.") return control = CarlaEgoVehicleControl() if twist == Twist(): # stop control.throttle = 0. control.brake = 1. control.steer = 0. else: if twist.linear.x > 0: control.throttle = min( TwistToVehicleControl.MAX_LON_ACCELERATION, twist.linear.x ) / TwistToVehicleControl.MAX_LON_ACCELERATION else: control.reverse = True control.throttle = max( -TwistToVehicleControl.MAX_LON_ACCELERATION, twist.linear.x ) / -TwistToVehicleControl.MAX_LON_ACCELERATION if twist.angular.z > 0: control.steer = -min(self.max_steering_angle, twist.angular.z) / \ self.max_steering_angle else: control.steer = -max(-self.max_steering_angle, twist.angular.z) / \ self.max_steering_angle try: self.pub.publish(control) except ROSException as e: if ros_ok(): self.logwarn("Error while publishing control: {}".format(e))
def _is_vehicle_hazard(self, vehicle_list, objects): """ Check if a given vehicle is an obstacle in our way. To this end we take into account the road and lane the target vehicle is on and run a geometry test to check if the target vehicle is under a certain distance in front of our ego vehicle. WARNING: This method is an approximation that could fail for very large vehicles, which center is actually on a different lane but their extension falls within the ego vehicle lane. :param vehicle_list: list of potential obstacle to check :return: a tuple given by (bool_flag, vehicle), where - bool_flag is True if there is a vehicle ahead blocking us and False otherwise - vehicle is the blocker object itself """ if self._vehicle_location is None: # no available self location yet return (False, None) ego_vehicle_location = get_service_request(GetWaypoint) ego_vehicle_location.location = self._vehicle_location ego_vehicle_waypoint = self.get_waypoint(ego_vehicle_location) if not ego_vehicle_waypoint: if ros_ok(): self.node.logwarn("Could not get waypoint for ego vehicle.") return (False, None) for target_vehicle_id in vehicle_list: # do not account for the ego vehicle if target_vehicle_id == self._vehicle_id: continue target_vehicle_location = None for elem in objects: if elem.id == target_vehicle_id: target_vehicle_location = elem.pose break if not target_vehicle_location: self.node.logwarn("Location of vehicle {} not found".format( target_vehicle_id)) continue # if the object is not in our lane it's not an obstacle request = get_service_request(GetWaypoint) request.location = target_vehicle_location.position target_vehicle_waypoint = self.get_waypoint(request) if not target_vehicle_waypoint: if ros_ok(): self.node.logwarn( "Could not get waypoint for target vehicle.") return (False, None) if target_vehicle_waypoint.road_id != ego_vehicle_waypoint.road_id or \ target_vehicle_waypoint.lane_id != ego_vehicle_waypoint.lane_id: continue if is_within_distance_ahead(target_vehicle_location.position, self._vehicle_location, math.degrees(self._vehicle_yaw), self._proximity_threshold): return (True, target_vehicle_id) return (False, None)
def _is_light_red_us_style(self, lights_list): # pylint: disable=too-many-branches """ This method is specialized to check US style traffic lights. :param lights_list: list containing TrafficLight objects :return: a tuple given by (bool_flag, traffic_light), where - bool_flag is True if there is a traffic light in RED affecting us and False otherwise - traffic_light is the object itself or None if there is no red traffic light affecting us """ if self._vehicle_location is None: # no available self location yet return (False, None) ego_vehicle_location = get_service_request(GetWaypoint) ego_vehicle_location.location = self._vehicle_location ego_vehicle_waypoint = self.get_waypoint(ego_vehicle_location) if not ego_vehicle_waypoint: if ros_ok(): self.node.logwarn("Could not get waypoint for ego vehicle.") return (False, None) if ego_vehicle_waypoint.is_junction: # It is too late. Do not block the intersection! Keep going! return (False, None) if self._target_route_point is not None: request = get_service_request(GetWaypoint) request.location = self._target_route_point target_waypoint = self.get_waypoint(request) if not target_waypoint: if ros_ok(): self.node.logwarn( "Could not get waypoint for target route point.") return (False, None) if target_waypoint.is_junction: min_angle = 180.0 sel_magnitude = 0.0 # pylint: disable=unused-variable sel_traffic_light = None for traffic_light in lights_list: loc = traffic_light[1] magnitude, angle = compute_magnitude_angle( loc.pose.position, ego_vehicle_location.location, math.degrees(self._vehicle_yaw)) if magnitude < 60.0 and angle < min(25.0, min_angle): sel_magnitude = magnitude sel_traffic_light = traffic_light[0] min_angle = angle if sel_traffic_light is not None: # print('=== Magnitude = {} | Angle = {} | ID = {}'.format( # sel_magnitude, min_angle, sel_traffic_light)) if self._last_traffic_light is None: self._last_traffic_light = sel_traffic_light state = None for status in self._traffic_lights: if status.id == sel_traffic_light: state = status.state break if state is None: self.node.logwarn( "Couldn't get state of traffic light {}".format( sel_traffic_light)) return (False, None) if state == CarlaTrafficLightStatus.RED: return (True, self._last_traffic_light) else: self._last_traffic_light = None return (False, None)
def setup_vehicles(self, vehicles): for vehicle in vehicles: if self.spawn_sensors_only is True: # spawn sensors of already spawned vehicles try: carla_id = vehicle["carla_id"] except KeyError as e: self.logerr( "Could not spawn sensors of vehicle {}, its carla ID is not known." .format(vehicle["id"])) break # spawn the vehicle's sensors self.setup_sensors(vehicle["sensors"], carla_id) else: spawn_object_request = get_service_request(SpawnObject) spawn_object_request.type = vehicle["type"] spawn_object_request.id = vehicle["id"] spawn_object_request.attach_to = 0 spawn_object_request.random_pose = False spawn_point = None # check if there's a spawn_point corresponding to this vehicle spawn_point_param = self.get_param( "spawn_point_" + vehicle["id"], None) spawn_param_used = False if (spawn_point_param is not None): # try to use spawn_point from parameters spawn_point = self.check_spawn_point_param( spawn_point_param) if spawn_point is None: self.logwarn( "{}: Could not use spawn point from parameters, ". format(vehicle["id"]) + "the spawn point from config file will be used.") else: self.loginfo("Spawn point from ros parameters") spawn_param_used = True if "spawn_point" in vehicle and spawn_param_used is False: # get spawn point from config file try: spawn_point = self.create_spawn_point( vehicle["spawn_point"]["x"], vehicle["spawn_point"]["y"], vehicle["spawn_point"]["z"], vehicle["spawn_point"]["roll"], vehicle["spawn_point"]["pitch"], vehicle["spawn_point"]["yaw"]) self.loginfo("Spawn point from configuration file") except KeyError as e: self.logerr( "{}: Could not use the spawn point from config file, " .format(vehicle["id"]) + "the mandatory attribute {} is missing, a random spawn point will be used" .format(e)) if spawn_point is None: # pose not specified, ask for a random one in the service call self.loginfo("Spawn point selected at random") spawn_point = Pose() # empty pose spawn_object_request.random_pose = True player_spawned = False while not player_spawned and ros_ok(): spawn_object_request.transform = spawn_point response_id = self.spawn_object(spawn_object_request) if response_id != -1: player_spawned = True self.players.append(response_id) # Set up the sensors try: self.setup_sensors(vehicle["sensors"], response_id) except KeyError: self.logwarn( "Object (type='{}', id='{}') has no 'sensors' field in his config file, none will be spawned." .format(spawn_object_request.type, spawn_object_request.id))
def setup_sensors(self, sensors, attached_vehicle_id=None): """ Create the sensors defined by the user and attach them to the vehicle (or not if global sensor) :param sensors: list of sensors :param attached_vehicle_id: id of vehicle to attach the sensors to :return actors: list of ids of objects created """ sensor_names = [] for sensor_spec in sensors: if not ros_ok(): break try: sensor_type = str(sensor_spec.pop("type")) sensor_id = str(sensor_spec.pop("id")) sensor_name = sensor_type + "/" + sensor_id if sensor_name in sensor_names: raise NameError sensor_names.append(sensor_name) if attached_vehicle_id is None and "pseudo" not in sensor_type: spawn_point = sensor_spec.pop("spawn_point") sensor_transform = self.create_spawn_point( spawn_point.pop("x"), spawn_point.pop("y"), spawn_point.pop("z"), spawn_point.pop("roll", 0.0), spawn_point.pop("pitch", 0.0), spawn_point.pop("yaw", 0.0)) else: # if sensor attached to a vehicle, or is a 'pseudo_actor', allow default pose spawn_point = sensor_spec.pop("spawn_point", 0) if spawn_point == 0: sensor_transform = self.create_spawn_point( 0.0, 0.0, 0.0, 0.0, 0.0, 0.0) else: sensor_transform = self.create_spawn_point( spawn_point.pop("x", 0.0), spawn_point.pop("y", 0.0), spawn_point.pop("z", 0.0), spawn_point.pop("roll", 0.0), spawn_point.pop("pitch", 0.0), spawn_point.pop("yaw", 0.0)) spawn_object_request = get_service_request(SpawnObject) spawn_object_request.type = sensor_type spawn_object_request.id = sensor_id spawn_object_request.attach_to = attached_vehicle_id if attached_vehicle_id is not None else 0 spawn_object_request.transform = sensor_transform spawn_object_request.random_pose = False # never set a random pose for a sensor attached_objects = [] for attribute, value in sensor_spec.items(): if attribute == "attached_objects": for attached_object in sensor_spec["attached_objects"]: attached_objects.append(attached_object) continue spawn_object_request.attributes.append( KeyValue(key=str(attribute), value=str(value))) response_id = self.spawn_object(spawn_object_request) if response_id == -1: raise RuntimeError(response.error_string) if attached_objects: # spawn the attached objects self.setup_sensors(attached_objects, response_id) if attached_vehicle_id is None: self.global_sensors.append(response_id) else: self.vehicles_sensors.append(response_id) except KeyError as e: self.logerr( "Sensor {} will not be spawned, the mandatory attribute {} is missing" .format(sensor_name, e)) continue except RuntimeError as e: self.logerr("Sensor {} will not be spawned: {}".format( sensor_name, e)) continue except NameError: self.logerr( "Sensor rolename '{}' is only allowed to be used once. The second one will be ignored." .format(sensor_id)) continue