public void SetCommand(double?engineTorque, double?brakePressue, double?steering, TransmissionGear?gear)
{
if (Settings.TestMode)
{
return;
}
if (steering.HasValue && Math.Abs(steering.Value) > TahoeParams.SW_max)
{
OperationalTrace.WriteWarning("steering out of range: received {0}, limit {1}", steering.Value, TahoeParams.SW_max);
}
//Console.WriteLine("command--steering: {0}, torque: {1}, brake: {2}, gear: {3}", steering, engineTorque, brakePressue, gear);
try {
dynamicsSim.SetSteeringBrakeThrottle(steering, engineTorque, brakePressue);
if (gear.HasValue)
{
dynamicsSim.SetTransmissionGear(gear.Value);
}
}
catch (SocketException) {
// get the DynamicsSimFacade
dynamicsSim = (DynamicsSimFacade)CommBuilder.GetObject(DynamicsSimFacade.ServiceName);
throw;
}
}
private static ArbiterAdvancedRemote GetRemote()
{
try {
return((ArbiterAdvancedRemote)CommBuilder.GetObject("ArbiterAdvancedRemote"));
}
catch (Exception ex) {
OperationalTrace.WriteWarning("error getting arbiter remote: {0}", ex);
return(null);
}
}
private static void BuildCommon()
{
// create the dataset
Services.Dataset = new Dataset.Source.DatasetSource("operational", "net.xml");
// initialize the communciations builder (gets object directory, channel factory)
CommBuilder.InitComm();
// create the tunable parameter table
using (FileStream fs = new FileStream("params.xml", FileMode.OpenOrCreate, FileAccess.Read)) {
Services.Params = new UrbanChallenge.OperationalUIService.Parameters.TunableParamTable(fs);
}
// build a default planar project
Services.Projection = new UrbanChallenge.Common.EarthModel.PlanarProjection(Settings.DefaultOrigin.X * Math.PI / 180.0, Settings.DefaultOrigin.Y * Math.PI / 180.0);
// create the relative pose builder
// 1000 entries should create 10 seconds of queue space
Services.RelativePose = new UrbanChallenge.Common.Pose.RelativeTransformBuilder(10000, true);
// build the tracked distance provider
Services.TrackedDistance = new OperationalLayer.Pose.TrackedDistance(10000);
// build the stopline provider
Services.Stopline = new OperationalLayer.Pose.StopLineService();
// build the absolute pose service
Services.AbsolutePose = new OperationalLayer.Pose.AbsolutePoseQueue(10000);
// build the absolute posterior pose service
Services.AbsolutePosteriorPose = new OperationalLayer.Pose.AbsolutePoseQueue(1000);
// build the lane model provider
Services.LaneModels = new OperationalLayer.RoadModel.LaneModelProvider();
// build the road model provider
Services.RoadModelProvider = new OperationalLayer.RoadModel.CombinedRoadModelProvider();
// Create the vehicle state provider
Services.StateProvider = new OperationalLayer.Pose.VehicleStateProvider();
// create the obstacle manager
Services.ObstacleManager = new ObstacleManager();
// build the UI service
Services.UIService = new UIService();
TimeoutMonitor.Initialize();
}
public FullSimTransport()
{
// initialize the car mode to unknown so that we raised the changed event immediately
lastCarMode = CarMode.Unknown;
// get the DynamicsSimFacade
dynamicsSim = (DynamicsSimFacade)CommBuilder.GetObject(DynamicsSimFacade.ServiceName);
// set the sim state channel
simStateChannel = CommBuilder.GetChannel(OperationalSimVehicleState.ChannelName);
obstacleChannel = CommBuilder.GetChannel(SceneEstimatorObstacleChannelNames.UntrackedClusterChannelName);
vehicleChannel = CommBuilder.GetChannel(SceneEstimatorObstacleChannelNames.TrackedClusterChannelName);
// add ourself as a listern
simStateChannel.Subscribe(this);
obstacleChannel.Subscribe(this);
vehicleChannel.Subscribe(this);
}
