Add VREP conversion methods for MRPT class objects

CMakeLists.txt

@@ -57,6 +57,7 @@ set(PROJECT_ROOT ${CMAKE_CURRENT_SOURCE_DIR})
 find_package(MRPT REQUIRED graphslam)
 message(STATUS "MRPT_VERSION: ${MRPT_VERSION}")
 
+# TODO - Link the mrpt libraries to the excecutable created by the VREP files 
 
 
 # User Options

mrpt_graphslam_2d/CMakeLists.txt

@@ -3,12 +3,18 @@ set(PROJECT_TEST_NAME ${PROJECT_NAME}_test)
 
 
 if (BUILD_TESTING)
-  set(test_sources "test/runTests.cpp")
-  add_executable(${PROJECT_TEST_NAME}
-    ${test_sources})
+  set(test_sources
+  	include/mrpt_bridge/vrep_conversion.h
+  	remoteApi/vrep_conversion.cpp  
+  	test/runTests.cpp
+  	test/convertTest.cpp
+  	)
+  add_executable(${PROJECT_TEST_NAME} ${test_sources})
   target_link_libraries(${PROJECT_TEST_NAME}
     gtest
-    pthread)
+    pthread
+    ${MRPT_LIBS}
+    )
   set_target_properties(
     ${PROJECT_TEST_NAME}
     PROPERTIES
@@ -29,15 +35,17 @@ if (EXISTS "$ENV{VREP}")
 
   set (VREP_PATH "$ENV{VREP}/programming" CACHE "The VREP build path" STRING)
 
-  set (SOURCES 
+  set (SOURCES
+  	include/mrpt_bridge/vrep_conversion.h 
     ${VREP_PATH}/remoteApi/extApi.h
     ${VREP_PATH}/remoteApi/extApiPlatform.h
     ${VREP_PATH}/include/shared_memory.h
     ${VREP_PATH}/remoteApi/extApi.c
     ${VREP_PATH}/common/shared_memory.c
-    ${VREP_PATH}/remoteApi/extApiPlatform.c)
+    ${VREP_PATH}/remoteApi/extApiPlatform.c
+    remoteApi/vrep_conversion.cpp)
 
-  include_directories (${VREP_PATH}/remoteApi ${VREP_PATH}/include)
+  include_directories (${VREP_PATH}/remoteApi ${VREP_PATH}/include include)
 
   add_definitions( -Dpthread )
   add_definitions (-DNON_MATLAB_PARSING)
@@ -46,13 +54,15 @@ if (EXISTS "$ENV{VREP}")
   add_library (test_Client SHARED ${SOURCES})
 
 
-  add_executable(bubbleRob_test remoteApi/bubbleRob_test.cpp)
+  add_executable(remoteApi_test remoteApi/remoteApi_test.cpp)
 
   target_link_libraries (
-  	bubbleRob_test 
+  	remoteApi_test 
   	test_Client 
   	pthread
-  	rt)
+  	rt
+    ${MRPT_LIBS}
+    )
 
 
 else()

mrpt_graphslam_2d/include/mrpt_bridge/vrep_conversion.h

@@ -0,0 +1,33 @@
+#ifndef VREP_CONVERSION_H
+#define VREP_CONVERSION_H
+
+#include <stdint.h>
+#include <string>
+
+#include <mrpt/version.h>
+
+#include "extApi.h"
+
+namespace mrpt
+{
+namespace obs
+{
+class CObservation2DRangeScan;
+}
+}
+
+namespace mrpt
+{
+namespace poses
+{
+class CPose3D;
+}
+}
+namespace vrep_bridge
+{ bool convert(const float range[], const simxFloat& _maxScanDistance,
+        const simxFloat& _scanningAngle,const mrpt::poses::CPose3D& _pose,mrpt::obs::CObservation2DRangeScan& _obj);
+
+  bool convert(const float position[3], const float quaternion[4],mrpt::poses::CPose3D& _pose);
+}  
+
+#endif  

mrpt_graphslam_2d/remoteApi/remoteApi_test.cpp

@@ -0,0 +1,62 @@
+#include <stdio.h>
+#include <stdlib.h>
+#include <math.h>
+#include <cstring>  
+#include <mrpt/obs/CObservation2DRangeScan.h>
+#include <mrpt/poses/CPose3D.h>
+#include "mrpt_bridge/vrep_conversion.h"
+extern "C" {
+    #include "extApi.h"
+}
+using namespace mrpt::obs;
+using namespace mrpt::poses;
+using namespace vrep_bridge;
+
+int main()
+{
+    simxFinish(-1); //close all existing connections.
+
+    int clientID=simxStart((simxChar*)"127.0.0.1",19997,true,true,2000,5);
+    simxInt handle;
+    simxFloat position[3],scanningAngle,maxScanDistance;
+    simxUChar* dataSignal;
+    int dataSignalSize,dataCount;
+    simxFloat quaternion[4];
+
+    if (clientID!=-1)
+    {
+        printf("Simulation started \n");
+
+        while (simxGetConnectionId(clientID)!=-1){
+            simxGetObjectHandle(clientID,"fastHokuyo",&handle,simx_opmode_streaming); //get object handlesimxGetObjectPosition(clientID,handle,-1,position,simx_opmode_streaming); // get object position
+            simxGetObjectPosition(clientID,handle,-1,position,simx_opmode_streaming); // get object orientation(returns euler angles)
+            simxGetObjectQuaternion(clientID,handle,-1,quaternion,simx_opmode_streaming);
+            simxGetStringSignal(clientID,"measuredDataAtThisTime",&dataSignal,&dataSignalSize,simx_opmode_streaming);
+            simxGetFloatSignal(clientID,"scanningAngle",&scanningAngle,simx_opmode_streaming);
+            simxGetFloatSignal(clientID,"maxScanDistance",&maxScanDistance,simx_opmode_streaming);
+            if (simxGetStringSignal(clientID,"measuredDataAtThisTime",&dataSignal,&dataSignalSize,simx_opmode_buffer)==simx_error_noerror)
+            {
+                dataCount=dataSignalSize/12;
+                float range[dataCount];
+                for (int i=0;i<dataCount;i++){
+                    float x =((float*)dataSignal)[3*i];
+                    float y =((float*)dataSignal)[3*i+1];
+                    range[i] = sqrt(x*x + y*y);
+                }
+                CPose3D sensor_pose;
+                bool pose_convert = convert(position,quaternion,sensor_pose);
+                CObservation2DRangeScan obj;
+                bool res = convert(range,maxScanDistance,scanningAngle,sensor_pose,obj);
+                printf("%d\n",res);
+
+            }
+            printf("Scanning angle : %f\n",scanningAngle ); 
+            printf("Handle : %u\n",handle); 
+            printf("Laser Position : (%f,%f,%f) \n",position[0],position[1],position[2]);    
+            extApi_sleepMs(100);             
+        }
+        simxFinish(clientID);
+    }
+    return(0);
+}
+

mrpt_graphslam_2d/remoteApi/vrep_conversion.cpp

@@ -0,0 +1,52 @@
+#include <stdio.h>
+#include <stdlib.h>
+#include <math.h>
+#include <cstring>  
+#include <mrpt/obs/CObservation2DRangeScan.h>
+#include "mrpt_bridge/vrep_conversion.h"
+#include <mrpt/math/CQuaternion.h>
+
+
+extern "C" {
+    #include "extApi.h"
+}
+
+using namespace mrpt::obs;
+namespace vrep_bridge {
+    bool convert(const float range[], const simxFloat& _maxScanDistance,
+        const simxFloat& _scanningAngle,const mrpt::poses::CPose3D& _pose,CObservation2DRangeScan& _obj){
+
+        int _dataCount = sizeof(range)/sizeof(range[0]);
+        _obj.rightToLeft = true;
+        //_obj.sensorLabel 
+        _obj.aperture = _scanningAngle;
+        _obj.maxRange = _maxScanDistance;
+        _obj.sensorPose = _pose; //pose is calculated from the position recieved from the simulation.
+        const double ang_step = _obj.aperture / (_dataCount - 1);
+        const double inv_ang_step = (_dataCount - 1) / _obj.aperture;
+        _obj.resizeScan(_dataCount);
+        for (std::size_t i = 0; i < _dataCount; i++)
+        {
+            int j = inv_ang_step * ang_step * i;
+            if (j < 0)
+                j += _dataCount;
+            else if (j >= _dataCount)
+                j -= _dataCount;  
+
+            const float r = range[j];
+            _obj.setScanRange(i, r);
+
+            const bool r_valid = ((_obj.scan[i] < (_maxScanDistance * 0.95)) && (_obj.scan[i] > 0));
+            _obj.setScanRangeValidity(i, r_valid);
+        }
+        return true;
+
+    }
+
+    bool convert(const float position[3], const float quaternion[4], mrpt::poses::CPose3D& _pose){
+        mrpt::math::CQuaternionDouble q = mrpt::math::CQuaternionDouble((double)quaternion[0],(double)quaternion[1]
+            ,(double)quaternion[2],(double)quaternion[3]);
+        _pose = mrpt::poses::CPose3D(q,(double)position[0],(double)position[1],(double)position[2]);
+        return true;
+    }
+}

mrpt_graphslam_2d/test/convertTest.cpp

@@ -0,0 +1,43 @@
+#include <gtest/gtest.h>
+#include <stdio.h>
+#include <stdlib.h>
+#include <math.h>
+#include <cstring>
+#define __STDC_FORMAT_MACROS  
+#include <mrpt/obs/CObservation2DRangeScan.h>
+#include <mrpt/poses/CPose3D.h>
+#include "mrpt_bridge/vrep_conversion.h"
+extern "C" {
+    #include "extApi.h"
+}
+using namespace mrpt::obs;
+using namespace mrpt::poses;
+using namespace vrep_bridge;
+
+
+TEST(ConvertTest, CObservation2DRangeScanTest)
+{
+
+	simxFloat position[3] ={1.00,1.00,1.00} ,scanningAngle = 4.18 ,maxScanDistance = 5;
+    simxUChar* dataSignal;
+    float range[] = {2.00,2.00,1.00};
+    simxFloat quaternion[4] = {0.5,0.5,0.5,0.5};
+    CPose3D sensor_pose;
+    bool res = convert(position,quaternion,sensor_pose);
+    CObservation2DRangeScan obj;
+    EXPECT_TRUE(convert(range,maxScanDistance,scanningAngle,sensor_pose,obj));
+    EXPECT_TRUE(obj.rightToLeft);
+	EXPECT_EQ(obj.aperture, scanningAngle);
+	EXPECT_EQ(obj.maxRange,maxScanDistance);
+	EXPECT_EQ(obj.sensorPose,sensor_pose);
+}
+
+TEST(ConvertTest,CPose3DTest){
+	simxFloat position[3] ={1.00,1.00,1.00};
+	simxFloat quaternion[4] = {0.5,0.5,0.5,0.5};
+	CPose3D pose;
+	EXPECT_TRUE(convert(position,quaternion,pose));
+	float pitch = asin(-2*((quaternion[0]*quaternion[2])-(quaternion[1]*quaternion[3])));
+	EXPECT_EQ((double)pitch,pose.pitch());
+}
+