def run_setup(self):
from monodrive import SimulatorConfiguration, VehicleConfiguration, Simulator
from monodrive.ui import GUI
from monodrive.networking.client import Client
# Simulator configuration defines network addresses for connecting to the simulator and material properties
simulator_config = SimulatorConfiguration('simulator.json')
self.client = Client((simulator_config.configuration["server_ip"],
simulator_config.configuration["server_port"]))
if not self.client.isconnected():
self.client.connect()
# Vehicle configuration defines ego vehicle configuration and the individual sensors configurations
vehicle_config = VehicleConfiguration('demo.json')
self.simulator = Simulator(self.client, simulator_config)
self.simulator.send_configuration()
self.map_data = self.simulator.request_map()
from monodrive import VehicleConfiguration
from monodrive.vehicles import LV_Vehicle
# Setup Ego Vehicle
vehicle_config = VehicleConfiguration('demo.json')
self.ego_vehicle = LV_Vehicle(simulator_config, vehicle_config,
self.restart_event, self.map_data)
# Send Radar Waveform
self.ego_vehicle.update_fmcw_in_config()
#helper = InterruptHelper()
self.simulator.restart_event.clear()
self.simulator.send_vehicle_configuration(vehicle_config)
time.sleep(1)
return True
def __init__(self, params):
"""
:param params: dict of parameters, see settings.yaml
:param rate: rate to query data from mono in Hz
"""
self.frames_per_episode = params['Framesperepisode']
# Simulator configuration defines network addresses for connecting to the simulator and material properties
simulator_config = SimulatorConfiguration(params['SimulatorConfig'])
# Vehicle configuration defines ego vehicle configuration and the individual sensors configurations
self.vehicle_config = VehicleConfiguration(params['VehicleConfig'])
self.simulator = Simulator(simulator_config)
self.vehicle = self.simulator.get_ego_vehicle(self.vehicle_config,
RosVehicle)
self.param_sensors = params.get('sensors', {})
self.tf_to_publish = []
self.msgs_to_publish = []
self.publishers = {}
# definitions useful for time
self.cur_time = rospy.Time.from_sec(
0) # at the beginning of simulation
self.mono_game_stamp = 0
self.mono_platform_stamp = 0
# creating handler to handle vehicles messages
self.player_handler = PlayerAgentHandler(
"player_vehicle", process_msg_fun=self.process_msg)
self.non_players_handler = NonPlayerAgentsHandler(
"vehicles", process_msg_fun=self.process_msg)
# creating handler for sensors
self.sensors = {}
print(self.param_sensors)
for t, sensors in self.param_sensors.items():
for id in sensors:
self.add_sensor(sensors[id])
__version__ = "1.0"
from monodrive import SimulatorConfiguration
from monodrive.vehicles import SimpleVehicle
from monodrive import Simulator
from monodrive.scenario import Scenario
if __name__ == "__main__":
# Simulator configuration defines network addresses for connecting to the simulator and material properties
simulator_config = SimulatorConfiguration('simulator.json')
scenario = Scenario('MDLaneChanger.xosc')
simulator = Simulator(simulator_config)
while True:
# Start Scenario
simulator.start_scenario(scenario, SimpleVehicle)
# Waits for the restart event to be set in the control process
simulator.restart_event.wait()
# Terminates vehicle and sensor processes
simulator.stop()
class MonoRosBridge(object):
"""
monoDrive Ros bridge
"""
def __init__(self, params):
"""
:param params: dict of parameters, see settings.yaml
:param rate: rate to query data from mono in Hz
"""
self.frames_per_episode = params['Framesperepisode']
# Simulator configuration defines network addresses for connecting to the simulator and material properties
simulator_config = SimulatorConfiguration(params['SimulatorConfig'])
# Vehicle configuration defines ego vehicle configuration and the individual sensors configurations
self.vehicle_config = VehicleConfiguration(params['VehicleConfig'])
self.simulator = Simulator(simulator_config)
self.vehicle = self.simulator.get_ego_vehicle(self.vehicle_config,
RosVehicle)
self.param_sensors = params.get('sensors', {})
self.tf_to_publish = []
self.msgs_to_publish = []
self.publishers = {}
# definitions useful for time
self.cur_time = rospy.Time.from_sec(
0) # at the beginning of simulation
self.mono_game_stamp = 0
self.mono_platform_stamp = 0
# creating handler to handle vehicles messages
self.player_handler = PlayerAgentHandler(
"player_vehicle", process_msg_fun=self.process_msg)
self.non_players_handler = NonPlayerAgentsHandler(
"vehicles", process_msg_fun=self.process_msg)
# creating handler for sensors
self.sensors = {}
print(self.param_sensors)
for t, sensors in self.param_sensors.items():
for id in sensors:
self.add_sensor(sensors[id])
# creating input controller listener
#self.input_controller = InputController()
def add_sensor(self, sensor):
rospy.loginfo("Adding sensor {}".format(sensor))
sensor_type = sensor['type']
id = sensor['id'] #'{0}.{1}'.format(sensor_type, sensor['id'])
sensor_handler = None
if sensor_type == 'Lidar':
sensor_handler = LidarHandler
elif sensor_type == 'Camera':
sensor_handler = CameraHandler
elif sensor_type == 'IMU':
sensor_handler = ImuHandler
elif sensor_type == 'GPS':
sensor_handler = GpsHandler
elif sensor_type == 'RPM':
sensor_handler = RpmHandler
elif sensor_type == 'BoundingBox':
sensor_handler = BoundingBoxHandler
elif sensor_type == 'Waypoint':
sensor_handler = WaypointHandler
if sensor_handler:
if self.sensors.get(sensor_type, None) is None:
self.sensors[sensor_type] = []
self.sensors[sensor_type].append(
sensor_handler(id,
self.vehicle.get_sensor(sensor_type, id),
process_msg_fun=self.process_msg))
else:
rospy.logerr(
"Unable to handle sensor {name} of type {sensor_type}".format(
sensor_type=sensor_type, name=id))
def on_shutdown(self):
rospy.loginfo("Shutdown requested")
def process_msg(self, topic=None, msg=None):
"""
Function used to process message
Here we create publisher if not yet created
Store the message in a list (waiting for their publication) with their associated publisher
Messages for /tf topics are handle differently in order to publish all transform in the same message
:param topic: topic to publish the message on
:param msg: monodrive_ros_bridge message
"""
#rospy.loginfo("publishing on {0}".format(topic))
if topic not in self.publishers:
if topic == 'tf':
self.publishers[topic] = rospy.Publisher(topic,
TFMessage,
queue_size=100)
else:
self.publishers[topic] = rospy.Publisher(topic,
type(msg),
queue_size=10)
if topic == 'tf':
# transform are merged in same message
self.tf_to_publish.append(msg)
else:
self.msgs_to_publish.append((self.publishers[topic], msg))
def send_msgs(self):
for publisher, msg in self.msgs_to_publish:
publisher.publish(msg)
self.msgs_to_publish = []
tf_msg = TFMessage(self.tf_to_publish)
self.publishers['tf'].publish(tf_msg)
self.tf_to_publish = []
def compute_cur_time_msg(self):
self.process_msg('clock', Clock(self.cur_time))
def run(self):
self.publishers['clock'] = rospy.Publisher("clock",
Clock,
queue_size=10)
rospy.loginfo('Starting Vehicle')
# Start the Vehicle process
#self.vehicle = self.simulator.start_vehicle(self.vehicle_config, RosVehicle)
for frame in count():
if (frame == self.frames_per_episode) or rospy.is_shutdown():
rospy.loginfo("----- end episode -----")
break
# rospy.loginfo("waiting for data")
# self.vehicle.sensor_data_ready.wait()
rospy.loginfo("processing data")
for sensor in self.vehicle.sensors:
if self.sensors.get(sensor.type, None):
rospy.loginfo("getting data from {0}{1}".format(
sensor.type, sensor.sensor_id))
processor = None
sensors = self.sensors[sensor.type]
for s in sensors:
if s.name == sensor.sensor_id:
processor = s
try:
data = sensor.q_vehicle.peek()
processor.process_sensor_data(data, self.cur_time)
except:
while not sensor.q_vehicle.empty():
data = sensor.q_vehicle.get()
processor.process_sensor_data(data, self.cur_time)
rospy.loginfo("publishing messages")
# publish all messages
self.send_msgs()
self.vehicle.sensor_data_ready.clear()
control_data = self.vehicle.drive(self.vehicle.sensors, None)
rospy.loginfo("--> {0}".format(control_data))
self.vehicle.send_control_data(control_data)
rospy.loginfo("waiting for data")
self.vehicle.sensor_data_ready.wait()
'''
measurements, sensor_data = self.client.read_data()
# handle time
self.mono_game_stamp = measurements.game_timestamp
self.cur_time = rospy.Time.from_sec(self.mono_game_stamp * 1e-3)
self.compute_cur_time_msg()
# handle agents
self.player_handler.process_msg(
measurements.player_measurements, cur_time=self.cur_time)
self.non_players_handler.process_msg(
measurements.non_player_agents, cur_time=self.cur_time)
# handle sensors
for name, data in sensor_data.items():
self.sensors[name].process_sensor_data(data, self.cur_time)
# publish all messages
self.send_msgs()
# handle control
if rospy.get_param('mono_autopilot', True):
control = measurements.player_measurements.autopilot_control
self.client.send_control(control)
else:
control = self.input_controller.cur_control
self.client.send_control(**control)
'''
# Waits for the restart event to be set in the control process
self.vehicle.restart_event.wait()
# Terminates control process
self.vehicle.stop()
def __enter__(self):
return self
def __exit__(self, exc_type, exc_val, exc_tb):
rospy.loginfo("Exiting Bridge")
return None
class TestDrive(object):
def __init__(self):
self.map_data = None
self.simulator = None
self.restart_event = None
self.gui = None
self.ego_vehicle = None
self.client = None
def start_ego(self):
self.ego_vehicle.start()
def get_simulator(self):
return self.simulator
def step(self, control):
self.ego_vehicle.step(self.client, {
'forward': control[0],
'right': control[1]
})
return control
def run_setup(self):
from monodrive import SimulatorConfiguration, VehicleConfiguration, Simulator
from monodrive.ui import GUI
from monodrive.networking.client import Client
# Simulator configuration defines network addresses for connecting to the simulator and material properties
simulator_config = SimulatorConfiguration('simulator.json')
self.client = Client((simulator_config.configuration["server_ip"],
simulator_config.configuration["server_port"]))
if not self.client.isconnected():
self.client.connect()
# Vehicle configuration defines ego vehicle configuration and the individual sensors configurations
vehicle_config = VehicleConfiguration('demo.json')
self.simulator = Simulator(self.client, simulator_config)
self.simulator.send_configuration()
self.map_data = self.simulator.request_map()
from monodrive import VehicleConfiguration
from monodrive.vehicles import LV_Vehicle
# Setup Ego Vehicle
vehicle_config = VehicleConfiguration('demo.json')
self.ego_vehicle = LV_Vehicle(simulator_config, vehicle_config,
self.restart_event, self.map_data)
# Send Radar Waveform
self.ego_vehicle.update_fmcw_in_config()
#helper = InterruptHelper()
self.simulator.restart_event.clear()
self.simulator.send_vehicle_configuration(vehicle_config)
time.sleep(1)
return True
def start_sensor_streams(self):
if not self.client.isconnected():
self.client.connect()
self.ego_vehicle.start_sensor_streaming(self.client)
def stop_sensor_streams(self):
if not self.client.isconnected():
self.client.connect()
self.ego_vehicle.stop_sensor_streaming(self.client)
def close_connection(self):
if self.client.isconnected():
self.client.disconnect()
self.client.stop()
def start_sensor_listening(self):
self.ego_vehicle.start_sensor_listening()
def start_gui(self):
#self.gui = GUI(self.simulator)
pass
def stop_all(self):
self.simulator.stop()
if __name__ == "__main__":
# Simulator configuration defines network addresses for connecting to the simulator and material properties
simulator_config = SimulatorConfiguration('simulator.json')
# Vehicle configuration defines ego vehicle configuration and the individual sensors configurations
vehicle_config = VehicleConfiguration('demo.json')
client = Client((simulator_config.configuration["server_ip"],
simulator_config.configuration["server_port"]))
if not client.isconnected():
client.connect()
simulator = Simulator(client, simulator_config)
simulator.setup_logger()
simulator.send_configuration()
time.sleep(2)
episodes = 1 # TODO... this should come from the scenario config
# Setup Ego Vehicle
from monodrive import VehicleConfiguration
#from monodrive.vehicles import LV_Vehicle
map_data = simulator.request_map()
#map_data = None
# Setup Ego Vehicle
if __name__ == "__main__":
signal.signal(signal.SIGINT, shutdown)
logging.basicConfig(level=logging.DEBUG)
simulator_config = SimulatorConfiguration('simulator.json')
vehicle_config = VehicleConfiguration('test.json')
client = Client((simulator_config.configuration["server_ip"],
simulator_config.configuration["server_port"]))
if not client.isconnected():
client.connect()
simulator = Simulator(client, simulator_config)
sensor_configuration = vehicle_config.sensor_configuration
vehicle_config.sensor_configuration = []
vehicle_config.configuration['sensors'] = []
logging.getLogger(LOG_CATEGORY).debug(
json.dumps(vehicle_config.configuration))
simulator.send_configuration()
simulator.send_vehicle_configuration(vehicle_config)
sensors = []
idx = 0
for sensor_config in sensor_configuration:
cmd = messaging.AttachSensorCommand('ego', sensor_config)
logging.getLogger(LOG_CATEGORY).debug('--> {0}'.format(
from monodrive.vehicles import SimpleVehicle
from monodrive.vehicles import TeleportVehicle
ManualDriveMode = True
if __name__ == "__main__":
# Simulator configuration defines network addresses for connecting to the simulator and material properties
simulator_config = SimulatorConfiguration('simulator.json')
# Vehicle configuration defines ego vehicle configuration and the individual sensors configurations
vehicle_config = VehicleConfiguration('demo.json')
simulator = Simulator(simulator_config)
simulator.send_configuration()
# time.sleep(30)
episodes = 1 # TODO... this should come from the scenario config
# Setup Ego Vehicle
if ManualDriveMode == True:
ego_vehicle = simulator.get_ego_vehicle(vehicle_config, TeleportVehicle)
else:
ego_vehicle = simulator.get_ego_vehicle(vehicle_config, SimpleVehicle)
# prctl.set_proctitle("monoDrive")
#
gui = None
while episodes > 0:
sys.exit(0)
if __name__ == "__main__":
signal.signal(signal.SIGINT, shutdown)
# set up logging
logging.basicConfig(level=logging.DEBUG)
args = parser.parse_args()
sim_config = SimulatorConfiguration(args.sim_config)
vehicle_config = VehicleConfiguration(args.vehicle_config)
sim_config.client_settings['logger']['sensor'] = 'debug'
sim_config.client_settings['logger']['network'] = 'debug'
simulator = Simulator(sim_config)
if args.include:
sensor_ids = args.include.split(',')
sensors = vehicle_config.sensor_configuration
list = []
for sensor in sensors:
if sensor['type'] in sensor_ids or sensor['type'] + ':' + sensor[
'id'] in sensor_ids:
sensor['sensor_process'] = True
list.append(sensor)
vehicle_config.sensor_configuration = list
vehicle_config.configuration['sensors'] = list