ROS support

softwares/lib/makePackages.bat

@@ -1,6 +1,7 @@
 @echo off
 
 copy /Y ..\..\src\lib\c_cpp\robotOne_matlab.h robotOne.h
+xcopy.exe "../../src/lib/matlab" "x64/matlab" /y /i /s /e
 
 copy /Y robotOne.h x64\matlab\robotOne\robotOne.h
 

softwares/lib/robotOne.h

@@ -27,12 +27,12 @@ int connectRobotOne(const char *address);
 int disconnectRobotOne();
 float versionRobotOne();
 float waitRobotOne();
-float get(char *name);
-float set(char *name, float value);
+float get(const char *name);
+float set(const char *name, float value);
 char* getData();
 float* getDataFloat();
-float setData(char *name, int size, char *data);
-float setDataFloat(char *name, int size, float *data);
+float setData(const char *name, int size, char *data);
+float setDataFloat(const char *name, int size, float *data);
 
 void initLidar(LidarData *lidarData, int size);
 int readLidar(LidarData *lidarData);

softwares/lib/robotOne.py

@@ -86,13 +86,13 @@ def captureCamera(rone):
 
 def cameraToPil(data):
     im = Image.frombuffer("RGB", (320, 240), data.contents, "raw", "RGB", 0, 1)
-    im = im.transpose(Image.FLIP_TOP_BOTTOM)
+    # im = im.transpose(Image.FLIP_TOP_BOTTOM)
     return im
 
 def cameraToCV(data):
     arr = np.ctypeslib.as_array((c_ubyte * 230400).from_address(addressof(data.contents)))
     im = arr.reshape(240,320,3)
-    im = cv2.flip( im, 0 )
+    # im = cv2.flip( im, 0 )
     im = cv2.cvtColor(im, cv2.COLOR_RGB2BGR)
     return im
 

softwares/lib/x64/matlab/robotOne/captureCamera.m

@@ -4,7 +4,9 @@
         data = getDataFloat(handler);
         setdatatype(data, 'uint8Ptr', sz);
         image = reshape(fliplr(reshape(data.value, 3, numel(data.value)/3).'),[320 240 3]);
-        image = flip(permute(image, [2 1 3]), 1);
+        % image = flip(permute(image, [2 1 3]), 1);
+	% image = flip(image, 2);
+	image = permute(image, [2 1 3]);
         tmp = image(:,:,1);
         image(:,:,1) = image(:,:,3);
         image(:,:,3) = tmp;

softwares/lib/x64/matlab/robotOne/robotOne.h

@@ -27,12 +27,12 @@ int connectRobotOne(const char *address);
 int disconnectRobotOne();
 float versionRobotOne();
 float waitRobotOne();
-float get(char *name);
-float set(char *name, float value);
+float get(const char *name);
+float set(const char *name, float value);
 char* getData();
 float* getDataFloat();
-float setData(char *name, int size, char *data);
-float setDataFloat(char *name, int size, float *data);
+float setData(const char *name, int size, char *data);
+float setDataFloat(const char *name, int size, float *data);
 
 void initLidar(LidarData *lidarData, int size);
 int readLidar(LidarData *lidarData);

softwares/lib/x64/robotOne.h

@@ -27,12 +27,12 @@ int connectRobotOne(const char *address);
 int disconnectRobotOne();
 float versionRobotOne();
 float waitRobotOne();
-float get(char *name);
-float set(char *name, float value);
+float get(const char *name);
+float set(const char *name, float value);
 char* getData();
 float* getDataFloat();
-float setData(char *name, int size, char *data);
-float setDataFloat(char *name, int size, float *data);
+float setData(const char *name, int size, char *data);
+float setDataFloat(const char *name, int size, float *data);
 
 void initLidar(LidarData *lidarData, int size);
 int readLidar(LidarData *lidarData);

softwares/lib/x86/robotOne.h

@@ -27,12 +27,12 @@ int connectRobotOne(const char *address);
 int disconnectRobotOne();
 float versionRobotOne();
 float waitRobotOne();
-float get(char *name);
-float set(char *name, float value);
+float get(const char *name);
+float set(const char *name, float value);
 char* getData();
 float* getDataFloat();
-float setData(char *name, int size, char *data);
-float setDataFloat(char *name, int size, float *data);
+float setData(const char *name, int size, char *data);
+float setDataFloat(const char *name, int size, float *data);
 
 void initLidar(LidarData *lidarData, int size);
 int readLidar(LidarData *lidarData);

src/app/Robot One/Assets/Scenes/MazeScene.unity

@@ -1723,6 +1723,11 @@ PrefabInstance:
   m_Modification:
     m_TransformParent: {fileID: 0}
     m_Modifications:
+    - target: {fileID: 114442352403073964, guid: 7585db4f5fd7b084789354da689180fd,
+        type: 3}
+      propertyPath: WorldCameraSelector
+      value: 
+      objectReference: {fileID: 1620028520}
     - target: {fileID: 114960102517224784, guid: 7585db4f5fd7b084789354da689180fd,
         type: 3}
       propertyPath: CameraTitle
@@ -1760,11 +1765,82 @@ PrefabInstance:
       propertyPath: m_RootOrder
       value: 1
       objectReference: {fileID: 0}
-    - target: {fileID: 114442352403073964, guid: 7585db4f5fd7b084789354da689180fd,
+    - target: {fileID: 4571757915829248, guid: 7585db4f5fd7b084789354da689180fd, type: 3}
+      propertyPath: m_LocalRotation.x
+      value: 0
+      objectReference: {fileID: 0}
+    - target: {fileID: 4571757915829248, guid: 7585db4f5fd7b084789354da689180fd, type: 3}
+      propertyPath: m_LocalRotation.y
+      value: 0
+      objectReference: {fileID: 0}
+    - target: {fileID: 4571757915829248, guid: 7585db4f5fd7b084789354da689180fd, type: 3}
+      propertyPath: m_LocalRotation.z
+      value: 1
+      objectReference: {fileID: 0}
+    - target: {fileID: 4571757915829248, guid: 7585db4f5fd7b084789354da689180fd, type: 3}
+      propertyPath: m_LocalRotation.w
+      value: 0
+      objectReference: {fileID: 0}
+    - target: {fileID: 4571757915829248, guid: 7585db4f5fd7b084789354da689180fd, type: 3}
+      propertyPath: m_LocalEulerAnglesHint.x
+      value: 0
+      objectReference: {fileID: 0}
+    - target: {fileID: 4571757915829248, guid: 7585db4f5fd7b084789354da689180fd, type: 3}
+      propertyPath: m_LocalEulerAnglesHint.y
+      value: 0
+      objectReference: {fileID: 0}
+    - target: {fileID: 4571757915829248, guid: 7585db4f5fd7b084789354da689180fd, type: 3}
+      propertyPath: m_LocalEulerAnglesHint.z
+      value: 180
+      objectReference: {fileID: 0}
+    - target: {fileID: 4571757915829248, guid: 7585db4f5fd7b084789354da689180fd, type: 3}
+      propertyPath: m_LocalScale.y
+      value: 1
+      objectReference: {fileID: 0}
+    - target: {fileID: 4571757915829248, guid: 7585db4f5fd7b084789354da689180fd, type: 3}
+      propertyPath: m_LocalScale.z
+      value: 0.6666667
+      objectReference: {fileID: 0}
+    - target: {fileID: 20362214678905868, guid: 7585db4f5fd7b084789354da689180fd,
         type: 3}
-      propertyPath: WorldCameraSelector
-      value: 
-      objectReference: {fileID: 1620028520}
+      propertyPath: m_NormalizedViewPortRect.y
+      value: 0
+      objectReference: {fileID: 0}
+    - target: {fileID: 20362214678905868, guid: 7585db4f5fd7b084789354da689180fd,
+        type: 3}
+      propertyPath: m_NormalizedViewPortRect.height
+      value: 1
+      objectReference: {fileID: 0}
+    - target: {fileID: 20362214678905868, guid: 7585db4f5fd7b084789354da689180fd,
+        type: 3}
+      propertyPath: m_projectionMatrixMode
+      value: 2
+      objectReference: {fileID: 0}
+    - target: {fileID: 20362214678905868, guid: 7585db4f5fd7b084789354da689180fd,
+        type: 3}
+      propertyPath: field of view
+      value: 60.000004
+      objectReference: {fileID: 0}
+    - target: {fileID: 20362214678905868, guid: 7585db4f5fd7b084789354da689180fd,
+        type: 3}
+      propertyPath: m_FocalLength
+      value: 44.167297
+      objectReference: {fileID: 0}
+    - target: {fileID: 20362214678905868, guid: 7585db4f5fd7b084789354da689180fd,
+        type: 3}
+      propertyPath: m_SensorSize.x
+      value: 70
+      objectReference: {fileID: 0}
+    - target: {fileID: 20362214678905868, guid: 7585db4f5fd7b084789354da689180fd,
+        type: 3}
+      propertyPath: m_SensorSize.y
+      value: 51
+      objectReference: {fileID: 0}
+    - target: {fileID: 20362214678905868, guid: 7585db4f5fd7b084789354da689180fd,
+        type: 3}
+      propertyPath: m_GateFitMode
+      value: 2
+      objectReference: {fileID: 0}
     m_RemovedComponents: []
   m_SourcePrefab: {fileID: 100100000, guid: 7585db4f5fd7b084789354da689180fd, type: 3}
 --- !u!114 &1620028520 stripped

src/app/Robot One/Assets/Scenes/World.unity

@@ -572,6 +572,11 @@ PrefabInstance:
   m_Modification:
     m_TransformParent: {fileID: 0}
     m_Modifications:
+    - target: {fileID: 114442352403073964, guid: 7585db4f5fd7b084789354da689180fd,
+        type: 3}
+      propertyPath: WorldCameraSelector
+      value: 
+      objectReference: {fileID: 334350765}
     - target: {fileID: 114960102517224784, guid: 7585db4f5fd7b084789354da689180fd,
         type: 3}
       propertyPath: CameraTitle
@@ -609,11 +614,51 @@ PrefabInstance:
       propertyPath: m_RootOrder
       value: 0
       objectReference: {fileID: 0}
-    - target: {fileID: 114442352403073964, guid: 7585db4f5fd7b084789354da689180fd,
+    - target: {fileID: 4571757915829248, guid: 7585db4f5fd7b084789354da689180fd, type: 3}
+      propertyPath: m_LocalRotation.x
+      value: 0
+      objectReference: {fileID: 0}
+    - target: {fileID: 4571757915829248, guid: 7585db4f5fd7b084789354da689180fd, type: 3}
+      propertyPath: m_LocalRotation.y
+      value: 0
+      objectReference: {fileID: 0}
+    - target: {fileID: 4571757915829248, guid: 7585db4f5fd7b084789354da689180fd, type: 3}
+      propertyPath: m_LocalRotation.z
+      value: 1
+      objectReference: {fileID: 0}
+    - target: {fileID: 4571757915829248, guid: 7585db4f5fd7b084789354da689180fd, type: 3}
+      propertyPath: m_LocalRotation.w
+      value: 0
+      objectReference: {fileID: 0}
+    - target: {fileID: 4571757915829248, guid: 7585db4f5fd7b084789354da689180fd, type: 3}
+      propertyPath: m_LocalEulerAnglesHint.z
+      value: 180
+      objectReference: {fileID: 0}
+    - target: {fileID: 20362214678905868, guid: 7585db4f5fd7b084789354da689180fd,
         type: 3}
-      propertyPath: WorldCameraSelector
-      value: 
-      objectReference: {fileID: 334350765}
+      propertyPath: m_projectionMatrixMode
+      value: 2
+      objectReference: {fileID: 0}
+    - target: {fileID: 20362214678905868, guid: 7585db4f5fd7b084789354da689180fd,
+        type: 3}
+      propertyPath: field of view
+      value: 60.000004
+      objectReference: {fileID: 0}
+    - target: {fileID: 20362214678905868, guid: 7585db4f5fd7b084789354da689180fd,
+        type: 3}
+      propertyPath: m_FocalLength
+      value: 44.167297
+      objectReference: {fileID: 0}
+    - target: {fileID: 20362214678905868, guid: 7585db4f5fd7b084789354da689180fd,
+        type: 3}
+      propertyPath: m_SensorSize.x
+      value: 70
+      objectReference: {fileID: 0}
+    - target: {fileID: 20362214678905868, guid: 7585db4f5fd7b084789354da689180fd,
+        type: 3}
+      propertyPath: m_SensorSize.y
+      value: 51
+      objectReference: {fileID: 0}
     m_RemovedComponents: []
   m_SourcePrefab: {fileID: 100100000, guid: 7585db4f5fd7b084789354da689180fd, type: 3}
 --- !u!114 &376016357 stripped

src/app/Robot One/Assets/Scripts/Server.cs

@@ -103,9 +103,30 @@ private void GetCameraImage()
         RenderTexture.active = RobotCamera.targetTexture;
         cameraImage.ReadPixels(cameraImageRect, 0, 0);
         RenderTexture.active = null;
+        cameraImage = Flip(cameraImage);
         cameraImageBytes = cameraImage.GetRawTextureData();
     }
 
+    Texture2D Flip(Texture2D original)
+    {
+        // We create a new texture so we don't change the old one!
+        Texture2D flip = new Texture2D(original.width, original.height, TextureFormat.RGB24, false);
+
+        // These for loops are for running through each individual pixel and then replacing them in the new texture.
+        for (int i = 0; i < flip.width; i++)
+        {
+            for (int j = 0; j < flip.height; j++)
+            {
+                flip.SetPixel(flip.width - i - 1, j, original.GetPixel(i, j));
+            }
+        }
+
+        // We apply the changes to our new texture
+        flip.Apply();
+        // Then we send it on our marry little way!
+        return flip;
+    }
+
     private void ServerThread()
     {
         while (running)

src/examples/cpp/localization.cpp

@@ -198,7 +198,7 @@ int main(int argc, char *argv[])
 
 			// Fix the image for opencv
 			cvtColor(rgb, bgr, COLOR_RGB2BGR);
-			flip(bgr, bgr, 0);
+			// flip(bgr, bgr, 0);
 
 			// Find boxes
 			ys.clear();

src/examples/cpp/plot.txt

@@ -0,0 +1,16 @@
+-5.53739e+15, -0.00170549, 0.929389, 0.00116042
+0.904454, -0.00361902, 1.44242, 0.00116365
+1.10074, -0.00274289, 1.80841, 0.00116596
+1.25168, -0.00432356, 2.1302, 0.00116798
+1.56728, -0.0083013, 2.60082, 0.00117095
+1.78624, -0.0104235, 2.91801, 0.00117295
+1.9681, -0.0132856, 3.20538, 0.00117476
+7.79966e+34, -0.0111647, 3.50371, 0.00117664
+2.33188, -0.0175456, 3.76667, 0.00117829
+-nan, 2.54703, 4.03176, 0.00117996
+230400, 2.61089, 4.07741, 0.00118025
+230400, 1.86284e+22, 4.29995, 0.00118165
+230400, 2.79416, 4.47776, 0.00118277
+230400, -1.20601e+38, 4.6438, 0.00118382
+230400, 2.99044, 4.80871, 0.00118486
+230400, -0.0175515, 4.98033, 0.00118594

src/lib/c_cpp/robotOne.cpp

@@ -101,7 +101,7 @@ extern_c float* getDataFloat()
 	return (float*)dataContainer;
 }
 
-extern_c float get(char *name)
+extern_c float get(const char *name)
 {
 	if (s == NULL || !s->isConnected())
 		return 0.0f;
@@ -120,7 +120,7 @@ extern_c float get(char *name)
 	return retValue;
 }
 
-extern_c float set(char *name, float value)
+extern_c float set(const char *name, float value)
 {
 	if (s == NULL || !s->isConnected())
 		return 0.0f;
@@ -142,7 +142,7 @@ extern_c float set(char *name, float value)
 	return retValue;
 }
 
-extern_c float setData(char *name, int size, char *data)
+extern_c float setData(const char *name, int size, char *data)
 {
 	if (s == NULL || !s->isConnected())
 		return 0.0f;
@@ -173,7 +173,7 @@ extern_c float setData(char *name, int size, char *data)
 	return retValue;
 }
 
-extern_c float setDataFloat(char *name, int size, float *data)
+extern_c float setDataFloat(const char *name, int size, float *data)
 {
 	if (s == NULL || !s->isConnected())
 		return 0.0f;

src/lib/c_cpp/robotOne.h

@@ -33,12 +33,12 @@ extern_c int connectRobotOne(const char *address);
 extern_c int disconnectRobotOne();
 extern_c float versionRobotOne();
 extern_c float waitRobotOne();
-extern_c float get(char *name);
-extern_c float set(char *name, float value);
+extern_c float get(const char *name);
+extern_c float set(const char *name, float value);
 extern_c char* getData();
 extern_c float* getDataFloat();
-extern_c float setData(char *name, int size, char *data);
-extern_c float setDataFloat(char *name, int size, float *data);
+extern_c float setData(const char *name, int size, char *data);
+extern_c float setDataFloat(const char *name, int size, float *data);
 
 extern_c void initLidar(LidarData *lidarData, int size);
 extern_c int readLidar(LidarData *lidarData);