/// <summary>
/// Replace Handler sets Dashboard State
/// </summary>
/// <param name="replace"></param>
IEnumerator<ITask> ReplaceHandler(Replace replace)
{
_state = replace.Body;
replace.ResponsePort.Post(dssp.DefaultReplaceResponseType.Instance);
yield break;
}
/// <summary>
/// Entry Point for the Dashboard Service
/// </summary>
protected override void Start()
{
// TT - Added code to create a default State if no
// config file exists
if (_state == null)
{
_state = new StateType();
_state.Log = false;
_state.LogFile = "";
_state.Machine = "";
_state.Port = 0;
}
// TT - Version 2 - The options "bag"
// This is tacky, but we need to set the default values
// in case there is no existing config.xml file
if (_state.Options == null)
{
_state.Options = new GUIOptions();
_state.Options.DeadZoneX = 80;
_state.Options.DeadZoneY = 80;
_state.Options.TranslateScaleFactor = 1.0;
_state.Options.RotateScaleFactor = 0.5;
_state.Options.ShowLRF = false;
_state.Options.ShowArm = false;
_state.Options.DisplayMap = false;
// Raul - Sept 2007
_state.Options.DisplaySonarMap = false;
// Updated in later versions with more options
// These values are in mm
_state.Options.RobotWidth = 300;
_state.Options.MaxLRFRange = 8192;
_state.Options.DriveDistance = 300;
// Speed is in mm/sec???
_state.Options.MotionSpeed = 100;
// Angle is in degrees
_state.Options.RotateAngle = 45;
// Camera update interval in milliseconds
// Note that this is only required for the
// simulated webcam because it does not provide
// updates when you subscribe
_state.Options.CameraInterval = 250;
// Raul - Version 9
_state.Options.SonarRange = 4000;
_state.Options.SonarTransducerAngularRange = 15.0f;
// Raul - Sonar transducer positions.
// This is only valid for a single sonar array of 8 transducers!!
_state.Options.SonarRadians = new double[8];
// Orientations of the P3DX frontal sonar transducers
_state.Options.SonarRadians[0] = (Math.PI * 90) / 180;
_state.Options.SonarRadians[1] = (Math.PI * 50) / 180;
_state.Options.SonarRadians[2] = (Math.PI * 30) / 180;
_state.Options.SonarRadians[3] = (Math.PI * 10) / 180;
_state.Options.SonarRadians[4] = -(Math.PI * 10) / 180;
_state.Options.SonarRadians[5] = -(Math.PI * 30) / 180;
_state.Options.SonarRadians[6] = -(Math.PI * 50) / 180;
_state.Options.SonarRadians[7] = -(Math.PI * 90) / 180;
}
if (_state.Options.CameraInterval < 100)
_state.Options.CameraInterval = 100;
// Handlers that need write or exclusive access to state go under
// the exclusive group. Handlers that need read or shared access, and can be
// concurrent to other readers, go to the concurrent group.
// Other internal ports can be included in interleave so you can coordinate
// intermediate computation with top level handlers.
Activate(Arbiter.Interleave(
new TeardownReceiverGroup
(
Arbiter.Receive<DsspDefaultDrop>(false, _mainPort, DropHandler)
),
new ExclusiveReceiverGroup
(
Arbiter.ReceiveWithIterator<Replace>(true, _mainPort, ReplaceHandler),
Arbiter.ReceiveWithIteratorFromPortSet<OnLoad>(true, _eventsPort, OnLoadHandler),
Arbiter.ReceiveFromPortSet<OnClosed>(true, _eventsPort, OnClosedHandler),
Arbiter.ReceiveWithIteratorFromPortSet<OnChangeJoystick>(true, _eventsPort, OnChangeJoystickHandler),
Arbiter.ReceiveFromPortSet<OnLogSetting>(true, _eventsPort, OnLogSettingHandler),
// TT - Added this handler for Connection parameters
Arbiter.ReceiveFromPortSet<OnConnectSetting>(true, _eventsPort, OnConnectSettingHandler),
// TT - Added this handler for Options
Arbiter.ReceiveFromPortSet<OnOptionSettings>(true, _eventsPort, OnOptionSettingsHandler)
// Raul - Nov 2008 - Skipping Webcam
// TT - Added this handler for Webcam
// Arbiter.ReceiveWithIteratorFromPortSet<OnConnectWebCam>(true, _eventsPort, OnConnectWebCamHandler),
// Arbiter.ReceiveWithIterator<cam.UpdateFrame>(true, _webCamNotify, CameraUpdateFrameHandler),
// Raul - Sept. Added this handler to disconnect cam - doesn't work yet
// Arbiter.ReceiveFromPortSet<OnDisconnectWebCam>(true, _eventsPort, OnDisconnectWebCamHandler),
// Raul - Handler for Cera Vision
// Arbiter.ReceiveWithIteratorFromPortSet<OnConnectVision>(true, _eventsPort, OnConnectVisionHandler),
// Arbiter.ReceiveFromPortSet<OnDisconnectVision>(true, _eventsPort, OnDisconnectVisionHandler)
),
new ConcurrentReceiverGroup
(
Arbiter.Receive<DsspDefaultLookup>(true,_mainPort,DefaultLookupHandler),
Arbiter.ReceiveWithIterator<Get>(true, _mainPort, GetHandler),
// TT Dec-2006 - Updated for V1.0
Arbiter.ReceiveWithIterator<game.Replace>(true, _gameControllerNotify, JoystickReplaceHandler),
// Arbiter.ReceiveWithIterator<sicklrf.Replace>(true, _laserNotify, OnLaserReplaceHandler),
Arbiter.ReceiveWithIterator<pxsonar.Replace>(true, _sonarNotify, OnSonarReplaceHandler),
Arbiter.ReceiveWithIteratorFromPortSet<OnConnect>(true, _eventsPort, OnConnectHandler),
Arbiter.ReceiveWithIterator<drive.Update>(true, _driveNotify, OnDriveUpdateNotificationHandler),
Arbiter.ReceiveWithIteratorFromPortSet<OnConnectMotor>(true, _eventsPort, OnConnectMotorHandler),
Arbiter.ReceiveWithIteratorFromPortSet<OnMove>(true, _eventsPort, OnMoveHandler),
// TT May-2007 - Added Rotate and Translate
Arbiter.ReceiveWithIteratorFromPortSet<OnRotate>(true, _eventsPort, OnRotateHandler),
Arbiter.ReceiveWithIteratorFromPortSet<OnTranslate>(true, _eventsPort, OnTranslateHandler),
Arbiter.ReceiveWithIteratorFromPortSet<OnEStop>(true, _eventsPort, OnEStopHandler),
Arbiter.ReceiveWithIteratorFromPortSet<OnStartService>(true, _eventsPort, OnStartServiceHandler),
Arbiter.ReceiveWithIteratorFromPortSet<OnConnectSickLRF>(true, _eventsPort, OnConnectSickLRFHandler),
Arbiter.ReceiveFromPortSet<OnDisconnectSickLRF>(true, _eventsPort, OnDisconnectSickLRFHandler),
// Raul - Sonar
Arbiter.ReceiveWithIteratorFromPortSet<OnConnectSonar>(true, _eventsPort, OnConnectSonarHandler),
Arbiter.ReceiveFromPortSet<OnDisconnectSonar>(true, _eventsPort, OnDisconnectSonarHandler),
// Raul - Sept 2007 - Cera Vision Concurrent Operations
// Raul - Nov 2007 - Skipping vision in this branch of code
// Arbiter.ReceiveWithIterator<vision.NotifyMotionImage>(true, _visionNotify, OnVisionMotionNotificationHandler),
// Arbiter.ReceiveWithIterator<vision.NotifyFaceDetection>(true, _visionNotify, OnVisionFaceNotificationHandler),
// Arbiter.ReceiveWithIterator<vision.NotifyHandGestureDetection>(true, _visionNotify, OnVisionHandNotificationHandler),
// Arbiter.ReceiveWithIterator<vision.NotifyObjectDetection>(true, _visionNotify, OnVisionObjectNotificationHandler),
Arbiter.ReceiveWithIteratorFromPortSet<OnConnectArticulatedArm>(true, _eventsPort, OnConnectArticulatedArmHandler),
Arbiter.ReceiveWithIteratorFromPortSet<OnApplyJointParameters>(true, _eventsPort, OnApplyJointParametersHandler)
)
));
DirectoryInsert();
WinFormsServicePort.Post(new RunForm(CreateForm));
}
/// <summary>
/// Entry Point for the Dashboard Service
/// </summary>
protected override void Start()
{
// TT - Added code to create a default State if no
// config file exists
if (_state == null)
{
_state = new StateType();
_state.Log = false;
_state.LogFile = "";
_state.Machine = "";
_state.Port = 0;
}
// TT - Version 2 - The options "bag"
// This is tacky, but we need to set the default values
// in case there is no existing config.xml file
if (_state.Options == null)
{
_state.Options = new GUIOptions();
_state.Options.DeadZoneX = 80;
_state.Options.DeadZoneY = 80;
_state.Options.TranslateScaleFactor = 1.0;
_state.Options.RotateScaleFactor = 0.5;
_state.Options.ShowLRF = false;
_state.Options.ShowArm = false;
_state.Options.DisplayMap = false;
// Raul - Sept 2007
_state.Options.DisplaySonarMap = false;
// Updated in later versions with more options
// These values are in mm
_state.Options.RobotWidth = 300;
_state.Options.MaxLRFRange = 8192;
_state.Options.DriveDistance = 300;
// Speed is in mm/sec???
_state.Options.MotionSpeed = 100;
// Angle is in degrees
_state.Options.RotateAngle = 45;
// Camera update interval in milliseconds
// Note that this is only required for the
// simulated webcam because it does not provide
// updates when you subscribe
_state.Options.CameraInterval = 250;
// Raul - Version 9
_state.Options.SonarRange = 4000;
_state.Options.SonarTransducerAngularRange = 15.0f;
// Raul - Sonar transducer positions.
// This is only valid for a single sonar array of 8 transducers!!
_state.Options.SonarRadians = new double[8];
// Orientations of the P3DX frontal sonar transducers
_state.Options.SonarRadians[0] = (Math.PI * 90) / 180;
_state.Options.SonarRadians[1] = (Math.PI * 50) / 180;
_state.Options.SonarRadians[2] = (Math.PI * 30) / 180;
_state.Options.SonarRadians[3] = (Math.PI * 10) / 180;
_state.Options.SonarRadians[4] = -(Math.PI * 10) / 180;
_state.Options.SonarRadians[5] = -(Math.PI * 30) / 180;
_state.Options.SonarRadians[6] = -(Math.PI * 50) / 180;
_state.Options.SonarRadians[7] = -(Math.PI * 90) / 180;
}
if (_state.Options.CameraInterval < 100)
_state.Options.CameraInterval = 100;
// Handlers that need write or exclusive access to state go under
// the exclusive group. Handlers that need read or shared access, and can be
// concurrent to other readers, go to the concurrent group.
// Other internal ports can be included in interleave so you can coordinate
// intermediate computation with top level handlers.
Activate(Arbiter.Interleave(
new TeardownReceiverGroup
(
Arbiter.Receive<DsspDefaultDrop>(false, _mainPort, DropHandler)
),
new ExclusiveReceiverGroup
(
Arbiter.ReceiveWithIterator<Replace>(true, _mainPort, ReplaceHandler),
Arbiter.ReceiveWithIteratorFromPortSet<OnLoad>(true, _eventsPort, OnLoadHandler),
Arbiter.ReceiveFromPortSet<OnClosed>(true, _eventsPort, OnClosedHandler),
Arbiter.ReceiveWithIteratorFromPortSet<OnChangeJoystick>(true, _eventsPort, OnChangeJoystickHandler),
Arbiter.ReceiveFromPortSet<OnLogSetting>(true, _eventsPort, OnLogSettingHandler),
// TT - Added this handler for Connection parameters
Arbiter.ReceiveFromPortSet<OnConnectSetting>(true, _eventsPort, OnConnectSettingHandler),
// TT - Added this handler for Options
Arbiter.ReceiveFromPortSet<OnOptionSettings>(true, _eventsPort, OnOptionSettingsHandler),
Arbiter.ReceiveWithIterator<cam.UpdateFrame>(true, _webCamNotify, CameraUpdateFrameHandler),
Arbiter.ReceiveWithIteratorFromPortSet<OnConnectPanTilt>(true, _eventsPort, OnConnectPanTiltHandler)
// Raul - Sept. Added this handler to disconnect cam - doesn't work yet
// Arbiter.ReceiveFromPortSet<OnDisconnectWebCam>(true, _eventsPort, OnDisconnectWebCamHandler),
// Raul - Handler for Cera Vision
// Arbiter.ReceiveWithIteratorFromPortSet<OnConnectVision>(true, _eventsPort, OnConnectVisionHandler),
// Arbiter.ReceiveFromPortSet<OnDisconnectVision>(true, _eventsPort, OnDisconnectVisionHandler)
),
new ConcurrentReceiverGroup
(
Arbiter.Receive<DsspDefaultLookup>(true,_mainPort,DefaultLookupHandler),
Arbiter.ReceiveWithIterator<Get>(true, _mainPort, GetHandler),
// TT Dec-2006 - Updated for V1.0
Arbiter.ReceiveWithIterator<game.Replace>(true, _gameControllerNotify, JoystickReplaceHandler),
Arbiter.ReceiveWithIterator<sicklrf.Replace>(true, _laserNotify, OnLaserReplaceHandler),
Arbiter.ReceiveWithIteratorFromPortSet<OnConnect>(true, _eventsPort, OnConnectHandler),
Arbiter.ReceiveWithIterator<drive.Update>(true, _driveNotify, OnDriveUpdateNotificationHandler),
Arbiter.ReceiveWithIteratorFromPortSet<OnConnectMotor>(true, _eventsPort, OnConnectMotorHandler),
Arbiter.ReceiveWithIteratorFromPortSet<OnMove>(true, _eventsPort, OnMoveHandler),
// TT May-2007 - Added Rotate and Translate
Arbiter.ReceiveWithIteratorFromPortSet<OnRotate>(true, _eventsPort, OnRotateHandler),
Arbiter.ReceiveWithIteratorFromPortSet<OnTranslate>(true, _eventsPort, OnTranslateHandler),
Arbiter.ReceiveWithIteratorFromPortSet<OnEStop>(true, _eventsPort, OnEStopHandler),
// Raul - LRF
Arbiter.ReceiveWithIteratorFromPortSet<OnStartService>(true, _eventsPort, OnStartServiceHandler),
Arbiter.ReceiveWithIteratorFromPortSet<OnConnectSickLRF>(true, _eventsPort, OnConnectSickLRFHandler),
Arbiter.ReceiveFromPortSet<OnDisconnectSickLRF>(true, _eventsPort, OnDisconnectSickLRFHandler),
// Raul - Sonar
Arbiter.ReceiveWithIteratorFromPortSet<OnConnectSonar>(true, _eventsPort, OnConnectSonarHandler),
Arbiter.ReceiveFromPortSet<OnDisconnectSonar>(true, _eventsPort, OnDisconnectSonarHandler),
Arbiter.ReceiveWithIterator<pxsonar.Replace>(true, _sonarNotify, OnSonarReplaceHandler),
// Raul - GPS
Arbiter.ReceiveWithIteratorFromPortSet<OnConnectGPS>(true, _eventsPort, OnConnectGPSHandler),
Arbiter.ReceiveFromPortSet<OnDisconnectGPS>(true, _eventsPort, OnDisconnectGPSHandler),
Arbiter.ReceiveWithIterator<pxGPS.Replace>(true, _gpsNotify, OnGPSReplaceHandler),
// Raul - Bumpers
Arbiter.ReceiveWithIteratorFromPortSet<OnConnectBumpers>(true, _eventsPort, OnConnectBumpersHandler),
Arbiter.ReceiveFromPortSet<OnDisconnectBumpers>(true, _eventsPort, OnDisconnectBumpersHandler),
Arbiter.ReceiveWithIterator<pxbumper.Replace>(true, _bumpersNotify, OnBumpersReplaceHandler),
Arbiter.ReceiveWithIterator<pxbumper.Update>(true, _bumpersNotify, OnBumperUpdateHandler),
Arbiter.ReceiveWithIteratorFromPortSet<OnConnectWebCam>(true, _eventsPort, OnConnectWebCamHandler),
Arbiter.ReceiveWithIteratorFromPortSet<OnPTMove>(true, _eventsPort, OnPanTiltMoveHandler)
)
));
DirectoryInsert();
_notificationTarget = new SimulationEnginePort();
EntitySubscribeRequestType req = new EntitySubscribeRequestType();
req.Name = "PursuitCam";
SimulationEngine.GlobalInstancePort.Subscribe(req, _notificationTarget);
// Set the Simulator camera view and resolution
UpdateCameraView view = new UpdateCameraView(new CameraView());
view.Body.EyePosition = new Vector3(1, 5, 1);
view.Body.LookAtPoint = new Vector3(0, 0, 0);
view.Body.XResolution = 640;
view.Body.YResolution = 480;
SimulationEngine.GlobalInstancePort.Post(view);
// get the current simulator configuration
_defaultConfig = new SimulatorConfiguration(true);
Activate(Arbiter.Choice(SimulationEngine.GlobalInstancePort.Query(_defaultConfig),
delegate(SimulatorConfiguration config)
{
_defaultConfig = config;
if (_driveControl != null)
WinFormsServicePort.FormInvoke(delegate() { _driveControl.SetHeadless(config.Headless); });
},
delegate(W3C.Soap.Fault fault)
{
}
));
// Add the winform message handler to the interleave
Activate(Arbiter.Interleave(
new TeardownReceiverGroup(),
new ExclusiveReceiverGroup
(
Arbiter.Receive<InsertSimulationEntity>(false, _notificationTarget, InsertEntityNotificationHandlerFirstTime),
Arbiter.ReceiveFromPortSet<OnSimulLoaded>(true, _eventsPort, OnSimulLoadedHandler),
Arbiter.ReceiveFromPortSet<OnSimulDrag>(true, _eventsPort, OnSimulDragHandler),
Arbiter.ReceiveFromPortSet<OnSimulZoom>(true, _eventsPort, OnSimulZoomHandler)
),
new ConcurrentReceiverGroup()
));
WinFormsServicePort.Post(new RunForm(CreateForm));
}
// TT - Change the parameters for Form constructor
public DriveControl(DriveControlEvents EventsPort, StateType state)
{
_eventsPort = EventsPort;
InitializeComponent();
simulImage.MouseMove += new MouseEventHandler(simulImage_MouseMove);
simulImage.MouseDown += new MouseEventHandler(simulImage_MouseDown);
_eventsPort.PostUnknownType(new OnSimulLoaded(this, this));
txtPort.ValidatingType = typeof(ushort);
// TT - Add initialization code based on the saved State
// Restore log file settings
txtLogFile.Text = state.LogFile;
if (state.Log)
{
if (state.LogFile != "")
{
chkLog.Checked = true;
// Initialize the log file
// Copied code from the Checkbox Click handler
txtLogFile.Enabled = !chkLog.Checked;
btnBrowse.Enabled = !chkLog.Checked;
_eventsPort.PostUnknownType(new OnLogSetting(this, chkLog.Checked, txtLogFile.Text));
}
else
chkLog.Checked = false;
}
else
chkLog.Checked = false;
// Set up the connection parameters
txtMachine.Text = state.Machine;
if (state.Port != 0)
txtPort.Text = state.Port.ToString();
// TT - Version 2 added all the options below
options = new GUIOptions();
options = state.Options;
this.StartPosition = FormStartPosition.Manual;
this.Location = new Point(options.WindowStartX, options.WindowStartY);
// The dead zone can't be negative, but it can be zero
options.DeadZoneX = Math.Abs(state.Options.DeadZoneX);
options.DeadZoneY = Math.Abs(state.Options.DeadZoneY);
// Just in case the scale factors have not been initialized
if (state.Options.TranslateScaleFactor == 0)
options.TranslateScaleFactor = 1.0;
else
options.TranslateScaleFactor = state.Options.TranslateScaleFactor;
if (state.Options.RotateScaleFactor == 0)
options.RotateScaleFactor = 0.5;
else
options.RotateScaleFactor = state.Options.RotateScaleFactor;
LRFPanelBaseY = groupBoxLRF.Top;
LRFPanelHeight = LRFPanelBaseY;
FormBaseHeight = this.Height - LRFPanelHeight;
ReformatForm();
}
