// Start is called before the first frame update
async void Start()
{
Timer.Init();
// set period for fixedUpdate() to be called where point cloud and pose can be shared
Time.fixedDeltaTime = (float)0.033333;
rosConnector = (RosSharp.RosBridgeClient.RosConnector)GetComponent(typeof(RosSharp.RosBridgeClient.RosConnector));
hololensPosePublisher = new HololensPosePublisher();
hololensPosePublisher.Init(ref rosConnector);
hololensPointCloudPublisher = new HololensPointCloudPublisher();
hololensPointCloudPublisher.Init(ref rosConnector);
markerPosePublisher = new MarkerPosePublisher();
markerPosePublisher.Init(ref rosConnector);
internalPointCloudQueue = new ConcurrentQueue <Vector3[]>();
robotPointCloudSubscriber = new RobotPointCloudSubscriber("/trimbot/alignedmap", 0, ref rosConnector, ref internalPointCloudQueue);
robotPointCloudGameObject = GameObject.Find("RobotPointCloud");
lastTimeRobotPointCloudReceived = Timer.SampleCurrentStopwatch();
robotTargetPosePublisher = new Publisher <PoseStamped>(ref rosConnector, "/trimbot/goal");
InputManager.Instance.PushFallbackInputHandler(this.gameObject);
#if NETFX_CORE
// initialize the camera data sources via Windows libraries
dataSourceGroup = new HololensRobotController.WindowsSensors.DataSourceGroup(ref rosConnector);
await dataSourceGroup.GetDataSources();
#endif
}
public void Init(ref RosSharp.RosBridgeClient.RosConnector rosConnector)
{
this.rosConnector = rosConnector;
publisher = new RosSharp.RosBridgeClient.NonMono.Publisher <RosSharp.RosBridgeClient.Messages.Sensor.PointCloud2>(ref this.rosConnector, "/hololens/" + Config.PointCloud);
spatialMappingManager = SpatialMappingManager.Instance;
spatialMappingManager.SurfaceObserver.TimeBetweenUpdates = Config.UnitySpatialMappingObserverTimeBetweenUpdates;
spatialMappingManager.SurfaceObserver.TrianglesPerCubicMeter = Config.UnitySpatialMappingObserverTrianglesPerCubicMeter;
spatialMappingManager.StartObserver(); // TODO: Check if offset is necessary, i.e. float startTime = SpatialMappingManager.Instance.StartTime;
spatialMappingManager.DrawVisualMeshes = Config.UnitySpatialMappingObserverDrawVisualMeshes;
CreatePointFieldArray();
}
public void Init(ref RosSharp.RosBridgeClient.RosConnector rosConnector)
{
this.rosConnector = rosConnector;
publisher = new RosSharp.RosBridgeClient.NonMono.Publisher <RosSharp.RosBridgeClient.Messages.Geometry.PoseStamped>(ref this.rosConnector, "/hololens/" + Config.MarkerPose);
GameObject aruwpController = GameObject.Find("ARUWPController");
if (aruwpController == null)
{
Debug.Log("MarkerPosePublisher: GameObject ARUWPController could not be found!");
Application.Quit();
}
else
{
marker = aruwpController.GetComponent <ARUWPMarker>();
}
if (marker == null)
{
Debug.Log("MarkerPosePublisher: GameObject ARUWPController has no ARUWPMarker component!");
Application.Quit();
}
}
void Start()
{
joystickCanvas.enabled = false;
cmdPublisher = new RosSharp.RosBridgeClient.NonMono.Publisher <Twist>(ref rosConnector, "/cmd_vel");
forwardLinear = new RosSharp.RosBridgeClient.Messages.Geometry.Vector3 {
x = 0.4f, y = 0.0f, z = 0.0f
};
backwardLinear = new RosSharp.RosBridgeClient.Messages.Geometry.Vector3 {
x = -0.4f, y = 0.0f, z = 0.0f
};
stopLinear = new RosSharp.RosBridgeClient.Messages.Geometry.Vector3 {
x = 0.0f, y = 0.0f, z = 0.0f
};
counterClockwiseAngular = new RosSharp.RosBridgeClient.Messages.Geometry.Vector3 {
x = 0.0f, y = 0.0f, z = 0.5f
};
clockwiseAngular = new RosSharp.RosBridgeClient.Messages.Geometry.Vector3 {
x = 0.0f, y = 0.0f, z = -0.5f
};
stopAngular = new RosSharp.RosBridgeClient.Messages.Geometry.Vector3 {
x = 0.0f, y = 0.0f, z = 0.0f
};
}
public void Init(ref RosSharp.RosBridgeClient.RosConnector rosConnector)
{
this.rosConnector = rosConnector;
publisher = new RosSharp.RosBridgeClient.NonMono.Publisher <RosSharp.RosBridgeClient.Messages.Geometry.PoseStamped>(ref this.rosConnector, "/hololens/" + Config.HololensPose);
}
/// <summary>
/// The Task to asynchronously initialize MediaCapture in UWP. The camera of HoloLens will
/// be configured to preview video of 896x504 at 30 fps, pixel format is NV12. MediaFrameReader
/// will be initialized and register the callback function OnFrameArrived to each video
/// frame. Note that this task does not start running the video preview, but configures the
/// running behavior. This task should be executed when ARUWPController status is
/// ARUWP_STATUS_CLEAN, and will change it to ARUWP_STATUS_VIDEO_INITIALIZED if no error
/// occurred. [internal use]
/// </summary>
/// <returns>Whether video pipeline is successfully initialized</returns>
public async Task <bool> InitializeMediaCaptureAsyncTask()
{
if (controller.status != ARUWP.ARUWP_STATUS_CLEAN)
{
Debug.Log(TAG + ": InitializeMediaCaptureAsyncTask() unsupported status");
return(false);
}
if (mediaCapture != null)
{
Debug.Log(TAG + ": InitializeMediaCaptureAsyncTask() fails because mediaCapture is not null");
return(false);
}
var allGroups = await MediaFrameSourceGroup.FindAllAsync();
foreach (var group in allGroups)
{
Debug.Log(group.DisplayName + ", " + group.Id);
}
if (allGroups.Count <= 0)
{
Debug.Log(TAG + ": InitializeMediaCaptureAsyncTask() fails because there is no MediaFrameSourceGroup");
return(false);
}
Config.SourceSelectionDictionary.TryGetValue(Config.Mono, out isPublishing);
if (isPublishing)
{
rosConnector = controller.rosConnector;
publisher = new RosSharp.RosBridgeClient.NonMono.Publisher <RosSharp.RosBridgeClient.Messages.Sensor.Image>(ref rosConnector, "/hololens/" + Config.Mono);
FrameReadyToPublish += PublishFrame;
}
// Initialize mediacapture with the source group.
mediaCapture = new MediaCapture();
var settings = new MediaCaptureInitializationSettings
{
SourceGroup = allGroups[0],
// This media capture can share streaming with other apps.
SharingMode = MediaCaptureSharingMode.SharedReadOnly,
// Only stream video and don't initialize audio capture devices.
StreamingCaptureMode = StreamingCaptureMode.Video,
// Set to CPU to ensure frames always contain CPU SoftwareBitmap images
// instead of preferring GPU D3DSurface images.
MemoryPreference = MediaCaptureMemoryPreference.Cpu
};
await mediaCapture.InitializeAsync(settings);
Debug.Log(TAG + ": MediaCapture is successfully initialized in shared mode.");
try
{
int targetVideoWidth, targetVideoHeight;
float targetVideoFrameRate;
switch (videoParameter)
{
case VideoParameter.Param1280x720x15:
targetVideoWidth = 1280;
targetVideoHeight = 720;
targetVideoFrameRate = 15.0f;
break;
case VideoParameter.Param1280x720x30:
targetVideoWidth = 1280;
targetVideoHeight = 720;
targetVideoFrameRate = 30.0f;
break;
case VideoParameter.Param1344x756x15:
targetVideoWidth = 1344;
targetVideoHeight = 756;
targetVideoFrameRate = 15.0f;
break;
case VideoParameter.Param1344x756x30:
targetVideoWidth = 1344;
targetVideoHeight = 756;
targetVideoFrameRate = 30.0f;
break;
case VideoParameter.Param896x504x15:
targetVideoWidth = 896;
targetVideoHeight = 504;
targetVideoFrameRate = 15.0f;
break;
case VideoParameter.Param896x504x30:
targetVideoWidth = 896;
targetVideoHeight = 504;
targetVideoFrameRate = 30.0f;
break;
default:
targetVideoWidth = 896;
targetVideoHeight = 504;
targetVideoFrameRate = 30.0f;
break;
}
var mediaFrameSourceVideoPreview = mediaCapture.FrameSources.Values.Single(x => x.Info.MediaStreamType == MediaStreamType.VideoPreview);
MediaFrameFormat targetResFormat = null;
float framerateDiffMin = 60f;
foreach (var f in mediaFrameSourceVideoPreview.SupportedFormats.OrderBy(x => x.VideoFormat.Width * x.VideoFormat.Height))
{
if (f.VideoFormat.Width == targetVideoWidth && f.VideoFormat.Height == targetVideoHeight)
{
if (targetResFormat == null)
{
targetResFormat = f;
framerateDiffMin = Mathf.Abs(f.FrameRate.Numerator / f.FrameRate.Denominator - targetVideoFrameRate);
}
else if (Mathf.Abs(f.FrameRate.Numerator / f.FrameRate.Denominator - targetVideoFrameRate) < framerateDiffMin)
{
targetResFormat = f;
framerateDiffMin = Mathf.Abs(f.FrameRate.Numerator / f.FrameRate.Denominator - targetVideoFrameRate);
}
}
}
if (targetResFormat == null)
{
targetResFormat = mediaFrameSourceVideoPreview.SupportedFormats[0];
Debug.Log(TAG + ": Unable to choose the selected format, fall back");
targetResFormat = mediaFrameSourceVideoPreview.SupportedFormats.OrderBy(x => x.VideoFormat.Width * x.VideoFormat.Height).FirstOrDefault();
}
await mediaFrameSourceVideoPreview.SetFormatAsync(targetResFormat);
frameReader = await mediaCapture.CreateFrameReaderAsync(mediaFrameSourceVideoPreview, targetResFormat.Subtype);
frameReader.FrameArrived += OnFrameArrived;
controller.frameWidth = Convert.ToInt32(targetResFormat.VideoFormat.Width);
controller.frameHeight = Convert.ToInt32(targetResFormat.VideoFormat.Height);
videoBufferSize = controller.frameWidth * controller.frameHeight * 4;
Debug.Log(TAG + ": FrameReader is successfully initialized, " + controller.frameWidth + "x" + controller.frameHeight +
", Framerate: " + targetResFormat.FrameRate.Numerator + "/" + targetResFormat.FrameRate.Denominator);
}
catch (Exception e)
{
Debug.Log(TAG + ": FrameReader is not initialized");
Debug.Log(TAG + ": Exception: " + e);
return(false);
}
if (isPublishing)
{
double fps;
if (Config.FrameRateDictionary.TryGetValue(Config.Mono, out fps))
{
publishPeriod = 1 / fps;
var bytesPerPixel = (int)HololensRobotController.WindowsSensors.CameraHandler.GetBytesPerPixel(MediaFrameSourceKind.Color);
int pixelBufferSize = (int)(controller.frameHeight * controller.frameWidth * bytesPerPixel);
publishRowSize = controller.frameWidth * bytesPerPixel;
publishBuffer = new byte[pixelBufferSize];
}
else
{
isPublishing = false;
}
}
controller.status = ARUWP.ARUWP_STATUS_VIDEO_INITIALIZED;
signalInitDone = true;
Debug.Log(TAG + ": InitializeMediaCaptureAsyncTask() is successful");
return(true);
}
