Add VREP conversion methods for MRPT class objects #13
There are no files selected for viewing
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -1,3 +1,26 @@ | ||
| PROJECT(${PROJECT_ROOT}_conversions) | ||
| set(CONVERT_TEST_NAME mrpt_vrep_bridge_conversions_test) | ||
|
|
||
| #Testing | ||
| ###################################################### | ||
|
|
||
| if (BUILD_TESTING) | ||
| set(convertTest_sources | ||
| include/vrep_conversion.h vrep_conversion.cpp test/runTests.cpp test/convertTest.cpp | ||
| ) | ||
| add_executable(${CONVERT_TEST_NAME} ${convertTest_sources}) | ||
| target_link_libraries(${CONVERT_TEST_NAME} | ||
| gtest pthread ${MRPT_LIBS} | ||
| ) | ||
| set_target_properties(${CONVERT_TEST_NAME} | ||
| PROPERTIES | ||
| RUNTIME_OUTPUT_DIRECTORY ${test_output_directory} | ||
| ) | ||
| add_test(NAME ${CONVERT_TEST_NAME} COMMAND | ||
| ${test_output_directory}/${CONVERT_TEST_NAME}) | ||
| endif() | ||
|
|
||
| set (CONVERSION_SOURCES include/vrep_conversion.h vrep_conversion.cpp) | ||
| include_directories (include) | ||
| add_library (conversion_lib SHARED ${CONVERSION_SOURCES}) | ||
| target_link_libraries( conversion_lib ${MRPT_LIBS} ) |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,51 @@ | ||
| #ifndef VREP_CONVERSION_H | ||
| #define VREP_CONVERSION_H | ||
|
|
||
| #include <cstdint> | ||
| #include <cstring> | ||
|
|
||
| namespace mrpt::obs | ||
| { | ||
| class CObservation2DRangeScan; | ||
| class CObservationOdometry; | ||
| } | ||
|
|
||
| namespace mrpt::poses | ||
| { | ||
| class CPose3D; | ||
| } | ||
|
|
||
| namespace vrep_bridge | ||
| { | ||
| /** | ||
| * Converts VREP laser scans into a CObservation2DRangeScan class object. It has the following arguments: | ||
| * @param[out] obj Converted CObservation2DRangeScan object. | ||
| * @param[in] _range[] This array contains all the laser scan measurements(distances). | ||
| * @param[in] _dataCount Contains the size of _range[]. | ||
| * @param[in] _maxScanDistance Contains maximum scan distance of scanner(simulation parameter) | ||
| * @param[in] _scanningAngle Contains scanning angle of scanner(simulation parameter) | ||
| * @param[in] _pose Sensor Pose. | ||
| */ | ||
| bool convert(const float range[], const int dataCount, const float& maxScanDistance, | ||
| const float& scanningAngle,const mrpt::poses::CPose3D& pose,mrpt::obs::CObservation2DRangeScan& obj); | ||
|
|
||
| /** | ||
| * Converts sensor position,quaternion into a CPose3D class object. It has the following arguments: | ||
| * @param[out] pose Converted CPose3D object. | ||
| * @param[in] position[3] This array contains x,y,z coordinates of the sensor. | ||
| * @param[in] quaternion[4] Contains the quaternion vector of the sensor. | ||
| * <i> Only position array is used for conversion to CPose3D in case _quaternion vector cannot be converted to CQuaternionDouble class<i> | ||
| */ | ||
| bool convert(const float position[3], const float quaternion[4],mrpt::poses::CPose3D& pose); | ||
|
|
||
| /** | ||
| * Converts pose,velocity and angular velocity value of an object into a CObservationOdometry class object. It has the following arguments: | ||
| * @param[out] obj Contains the converted CObservationOdometry object. | ||
| * @param[in] _pose3D Contains sensor pose.. | ||
| * @param[in] _vel[3] Contains x,y,z velocity value of the sensor. | ||
| * @param[in] _angularvelocity[3] Contains angular velocity across x,y,z axis. | ||
| */ | ||
| bool convert(const mrpt::poses::CPose3D& pose3D,const float vel[3], const float angularvelocity[3], mrpt::obs::CObservationOdometry& obj); | ||
| } | ||
|
|
||
| #endif |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,135 @@ | ||
| #include <gtest/gtest.h> | ||
| #include <stdio.h> | ||
| #include <stdlib.h> | ||
| #include <math.h> | ||
| #include <cstring> | ||
| #include "mrpt/obs/CObservation2DRangeScan.h" | ||
| #include "mrpt/obs/CObservationOdometry.h" | ||
| #include "mrpt/poses/CPose3D.h" | ||
| #include "vrep_conversion.h" | ||
| using namespace mrpt::obs; | ||
| using namespace mrpt::poses; | ||
| using namespace vrep_bridge; | ||
|
|
||
| /** | ||
| * This unit test tests the CObservation2DRangeScan conversion method. It tests the following parameters of the object created with the functions parameters. | ||
| *- <b>obj.aperture[]</b> -> scanningAngle | ||
| *- <b>obj.maxRange</b> -> maxScanDistance | ||
| *- <b>obj.sensorPose</b> -> sensor_pose | ||
| * | ||
| */ | ||
| TEST(ConvertTest, CObservation2DRangeScanTest) | ||
| { | ||
| float position[3] ={0.175f,0.00f,0.035f} ,scanningAngle = 4.1867f ,maxScanDistance = 5.00f; | ||
| float range[] = | ||
| { | ||
| 0.522596f ,0.520694f ,0.5189f ,0.516002f ,0.515378f ,0.514868f ,0.512759f ,0.513314f ,0.511191f ,0.512639f , | ||
| 0.51111f ,0.512957f ,0.512058f ,0.515077f ,0.514384f ,0.514165f ,0.518676f ,0.518822f ,0.519768f ,0.52625f , | ||
| 0.528463f ,0.530869f ,0.534459f ,0.552093f ,0.514051f ,0.51159f ,0.50079f ,0.494321f ,0.490789f ,0.488556f , | ||
| 0.485548f ,0.483604f ,0.481487f ,0.480994f ,0.480249f ,0.478227f ,0.478654f ,0.478947f ,0.477009f ,0.478284f , | ||
| 0.479595f ,0.477731f ,0.479953f ,0.482361f ,0.480562f ,0.484045f ,0.482289f ,0.486024f ,0.491505f ,0.489796f , | ||
| 0.496636f ,0.494958f ,0.50294f ,0.523664f ,0.521971f ,0.463143f ,0.462066f ,0.44504f ,0.444043f ,0.436001f , | ||
| 0.435061f ,0.429915f ,0.429025f ,0.425276f ,0.424431f ,0.422605f ,0.4218f ,0.420262f ,0.419496f ,0.418933f , | ||
| 0.418204f ,0.418977f ,0.418282f ,0.418851f ,0.41819f ,0.417546f ,0.419639f ,0.419027f ,0.421455f ,0.420873f , | ||
| 0.423868f ,0.423317f ,0.427758f ,0.427235f ,0.432961f ,0.432465f ,0.439457f ,0.438988f ,0.45503f ,0.45458f , | ||
| 0.411491f ,0.41146f ,0.400607f ,0.400607f ,0.400622f ,0.393857f ,0.393901f ,0.38893f ,0.389004f ,0.385895f , | ||
| 0.385997f ,0.383287f ,0.383418f ,0.38166f ,0.381818f ,0.381992f ,0.380751f ,0.380953f ,0.380026f ,0.380256f , | ||
| 0.38067f ,0.38093f ,0.38204f ,0.38233f ,0.383886f ,0.384206f ,0.384542f ,0.387214f ,0.387583f ,0.39055f , | ||
| 0.390952f ,0.39616f ,0.396598f ,0.40411f ,0.404589f ,0.418109f ,0.418637f ,0.385371f ,0.386406f ,0.375865f , | ||
| 0.376908f ,0.370591f ,0.371654f ,0.366174f ,0.361973f ,0.363061f ,0.359515f ,0.360629f ,0.357786f ,0.35893f , | ||
| 0.356348f ,0.354518f ,0.355702f ,0.353672f ,0.354889f ,0.35391f ,0.355163f ,0.354233f ,0.353274f ,0.354579f , | ||
| 0.354433f ,0.354312f ,0.355676f ,0.355701f ,0.357108f ,0.357816f ,0.358547f ,0.360023f ,0.361401f ,0.363093f , | ||
| 0.364649f ,0.367767f ,0.370789f ,0.372441f ,0.377908f ,0.386399f ,0.390203f ,0.383448f ,0.378072f ,0.374658f , | ||
| 0.371442f ,0.369523f ,0.36294f ,0.361983f ,0.361103f ,0.360299f ,0.359571f ,0.355979f ,0.355883f ,0.355851f , | ||
| 0.35292f ,0.353389f ,0.353917f ,0.351983f ,0.352645f ,0.351138f ,0.352226f ,0.351245f ,0.35246f ,0.351627f , | ||
| 0.353249f ,0.353141f ,0.353116f ,0.354935f ,0.355285f ,0.356074f ,0.358392f ,0.359699f ,0.361776f ,0.364492f , | ||
| 0.364054f ,0.367074f ,0.371484f ,0.371965f ,0.377694f ,0.379585f ,0.39053f ,0.406653f ,0.406642f ,0.39847f , | ||
| 0.393974f ,0.389529f ,0.386683f ,0.383892f ,0.382122f ,0.380322f ,0.378562f ,0.377526f ,0.376507f ,0.375526f , | ||
| 0.373195f ,0.372283f ,0.372216f ,0.371937f ,0.371696f ,0.371887f ,0.372259f ,0.372676f ,0.370578f ,0.371062f , | ||
| 0.372221f ,0.373365f ,0.374565f ,0.372583f ,0.374369f ,0.376379f ,0.378465f ,0.381356f ,0.379459f ,0.382617f , | ||
| 0.385892f ,0.384044f ,0.389294f ,0.395425f ,0.403015f ,0.401181f ,0.416441f ,0.359556f ,0.358512f ,0.347937f , | ||
| 0.346959f ,0.340757f ,0.339829f ,0.336336f ,0.335451f ,0.332241f ,0.331396f ,0.329413f ,0.328604f ,0.326725f , | ||
| 0.325805f ,0.325047f ,0.324599f ,0.323872f ,0.323844f ,0.323146f ,0.323147f ,0.322477f ,0.323466f ,0.322823f , | ||
| 0.32367f ,0.323053f ,0.32456f ,0.323969f ,0.325721f ,0.325154f ,0.326954f ,0.326412f ,0.329684f ,0.329164f , | ||
| 0.328657f ,0.331589f ,0.331105f ,0.335941f ,0.335478f ,0.341327f ,0.340883f ,0.347614f ,0.347189f ,0.365663f , | ||
| 0.365245f ,0.409723f ,0.40977f ,0.381707f ,0.381779f ,0.374049f ,0.374148f ,0.367739f ,0.367864f ,0.363015f , | ||
| 0.363166f ,0.363331f ,0.360174f ,0.360365f ,0.35757f ,0.357786f ,0.355923f ,0.356166f ,0.354637f ,0.354906f , | ||
| 0.354103f ,0.354399f ,0.354708f ,0.354155f ,0.354492f ,0.354687f ,0.355052f ,0.356233f ,0.356627f ,0.357861f , | ||
| 0.358285f ,0.360907f ,0.361363f ,0.363851f ,0.364339f ,0.369091f ,0.369615f ,0.376163f ,0.376727f ,0.377308f , | ||
| 0.388464f ,0.389094f ,0.45283f ,0.454046f ,0.427938f ,0.429126f ,0.421014f ,0.422221f ,0.415221f ,0.409613f , | ||
| 0.410844f ,0.407127f ,0.408389f ,0.404664f ,0.405957f ,0.403514f ,0.400878f ,0.402217f ,0.40078f ,0.402159f , | ||
| 0.40078f ,0.402199f ,0.401146f ,0.400661f ,0.40214f ,0.401625f ,0.40204f ,0.403587f ,0.403612f ,0.405208f , | ||
| 0.406656f ,0.408131f ,0.409811f ,0.412816f ,0.415607f ,0.417388f ,0.423047f ,0.431611f ,0.433534f ,0.477153f , | ||
| 0.470957f ,0.46552f ,0.462042f ,0.458759f ,0.456723f ,0.454801f ,0.453091f ,0.45212f ,0.446713f ,0.446509f , | ||
| 0.446379f ,0.446323f ,0.446342f ,0.443543f ,0.444175f ,0.444877f ,0.442981f ,0.444239f ,0.445565f ,0.444401f , | ||
| 0.446282f ,0.446131f ,0.448157f ,0.44866f ,0.451289f ,0.452373f ,0.455437f ,0.457836f ,0.460719f ,0.465214f , | ||
| 0.471546f ,0.485626f , | ||
| }; | ||
|
There was a problem hiding this comment. Have you tested that these values are written correctly or are you still working on this? There was a problem hiding this comment. Yes these values were received from vrep and they have been tested There was a problem hiding this comment. No, that's not what I mean. I want to have the values, one by one checked whether they are written correctly in the CObservation2DRangeScan object. Loop through that array and check each value, one by one. |
||
| int dataCount = 402; | ||
| float quaternion[4] = {-0.000028f,0.00f,0.00f,1.00f}; | ||
| CPose3D sensor_pose; | ||
| bool res = convert(position,quaternion,sensor_pose); | ||
| CObservation2DRangeScan obj; | ||
| EXPECT_TRUE(convert(range,dataCount,maxScanDistance,scanningAngle,sensor_pose,obj)); | ||
| EXPECT_TRUE(obj.rightToLeft); | ||
| EXPECT_FLOAT_EQ(obj.aperture, scanningAngle); | ||
| EXPECT_FLOAT_EQ(obj.maxRange,maxScanDistance); | ||
| EXPECT_EQ(obj.sensorPose,sensor_pose); | ||
| for (std::size_t i = 0; i < dataCount; i++) | ||
| { | ||
| const float r = range[i]; | ||
| const bool r_valid = ((obj.scan[i] < (maxScanDistance * 0.95)) && (obj.scan[i] > 0)); | ||
| EXPECT_FLOAT_EQ(obj.getScanRange(i),r); | ||
| EXPECT_EQ(obj.getScanRangeValidity(i),r_valid); | ||
| EXPECT_EQ(obj.getScanSize(),dataCount); | ||
| } | ||
| } | ||
| /** | ||
| * This unit test tests the yaw, pitch, roll values of the converted CPose3D object with the yaw,pitch,roll values calculated from the quaternion array. | ||
| * | ||
| */ | ||
| TEST(ConvertTest,CPose3DTest) | ||
| { | ||
| float position[3] ={0.175f,0.00f,0.035f}; | ||
| float quaternion[4] = {-0.000028f,0.00f,0.00f,1.00f}; | ||
| float yaw,pitch,roll; | ||
| if((mrpt::square(quaternion[0])+mrpt::square(quaternion[1])+mrpt::square(quaternion[2])+mrpt::square(quaternion[3]))-1 <1e-3) | ||
| {// Check to ensure intialization of data is normalized | ||
| for(int i=0;i<4;i++) | ||
| quaternion[i] = 0; | ||
| yaw = 0; | ||
| pitch = 0; | ||
| roll = 0; | ||
| } | ||
| else | ||
| { | ||
| yaw = atan((2*((quaternion[1]*quaternion[0])+(quaternion[3]*quaternion[2])))/ | ||
| (mrpt::square(quaternion[3])-mrpt::square(quaternion[2])-mrpt::square(quaternion[1])+mrpt::square(quaternion[0]))); | ||
| pitch = asin(-2*((quaternion[0]*quaternion[2])-(quaternion[1]*quaternion[3]))); | ||
| roll = atan((2*((quaternion[1]*quaternion[2])+(quaternion[3]*quaternion[0])))/ | ||
| (mrpt::square(quaternion[3])+mrpt::square(quaternion[2])-mrpt::square(quaternion[1])-mrpt::square(quaternion[0]))); | ||
| } | ||
| CPose3D pose; | ||
| EXPECT_TRUE(convert(position,quaternion,pose)); | ||
| EXPECT_DOUBLE_EQ(static_cast<double>(yaw),pose.yaw()); | ||
| EXPECT_DOUBLE_EQ(static_cast<double>(roll),pose.roll()); | ||
| EXPECT_DOUBLE_EQ(static_cast<double>(pitch),pose.pitch()); | ||
| } | ||
| /** | ||
| * This unit test tests the pose of the converted CObservationOdometry object with the pose of the sensor calculated from the position and quaternion values recieved from the simulator. | ||
| * | ||
| */ | ||
| TEST(ConvertTest,CObservationOdometryTest) | ||
| { | ||
| float position[3] ={0.175f,0.00f,0.035f}; | ||
| float quaternion[4] = {-0.000028f,0.00f,0.00f,1.00f}; | ||
| float vel[3] ={1.00,1.00,1.00}; | ||
| float ang_vel[4] = {0.5,0.5,0.5}; | ||
| CObservationOdometry odometry; | ||
| CPose3D pose; | ||
| bool res = convert(position,quaternion,pose); | ||
| CPose2D pose_2D = CPose2D(pose); | ||
| EXPECT_TRUE(convert(pose,vel,ang_vel,odometry)); | ||
| EXPECT_TRUE(odometry.hasVelocities); | ||
| EXPECT_EQ(pose_2D,odometry.odometry); | ||
| } | ||
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,8 @@ | ||
| #include "gtest/gtest.h" | ||
|
|
||
| int main(int argc, char** argv) | ||
| { | ||
| testing::InitGoogleTest(&argc, argv); | ||
| return RUN_ALL_TESTS(); | ||
| } | ||
|
|
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,58 @@ | ||
| #include <cstdio> | ||
| #include <cstdlib> | ||
| #include <cmath> | ||
| #include <cstring> | ||
| #include "mrpt/obs/CObservation2DRangeScan.h" | ||
| #include "vrep_conversion.h" | ||
| #include "mrpt/math/CQuaternion.h" | ||
| #include "mrpt/obs/CObservationOdometry.h" | ||
|
|
||
| using namespace mrpt::obs; | ||
| using namespace mrpt::poses; | ||
| using namespace mrpt::math; | ||
|
|
||
| namespace vrep_bridge | ||
| { | ||
| bool convert(const float range[], const int dataCount, const float& maxScanDistance, | ||
| const float& scanningAngle,const mrpt::poses::CPose3D& pose,mrpt::obs::CObservation2DRangeScan& obj) | ||
| { obj.rightToLeft = true; | ||
| obj.aperture = scanningAngle; | ||
| obj.maxRange = maxScanDistance; | ||
| obj.sensorPose = pose; | ||
| obj.resizeScan(dataCount); | ||
| for (std::size_t i = 0; i < dataCount; i++) | ||
| { | ||
| const float r = range[i]; | ||
| 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) | ||
| { | ||
| if((mrpt::square(quaternion[0])+mrpt::square(quaternion[1])+mrpt::square(quaternion[2])+mrpt::square(quaternion[3]))-1 <1e-3) | ||
| { | ||
| pose = CPose3D(static_cast<double>(position[0]),static_cast<double>(position[1]),static_cast<double>(position[2])); | ||
| } | ||
| else | ||
| { | ||
| CQuaternionDouble q = mrpt::math::CQuaternionDouble(static_cast<double>(quaternion[0]),static_cast<double>(quaternion[1]) | ||
| ,static_cast<double>(quaternion[2]),static_cast<double>(quaternion[3])); | ||
| pose = CPose3D(q,static_cast<double>(position[0]),static_cast<double>(position[1]),static_cast<double>(position[2])); | ||
| } | ||
| return true; | ||
| } | ||
|
|
||
| bool convert(const mrpt::poses::CPose3D& pose3D, const float vel[3], const float angularvelocity[3],mrpt::obs::CObservationOdometry& obj) | ||
| { | ||
| CPose2D pose2D = CPose2D(pose3D); | ||
| obj.odometry = pose2D; | ||
| obj.hasEncodersInfo = false; | ||
| obj.hasVelocities = true; | ||
| TTwist2D tTwist = TTwist2D(static_cast<double>(vel[0]),static_cast<double>(vel[1]),static_cast<double>(angularvelocity[2])); // velocity in 2D + angular velocity along z | ||
| obj.velocityLocal = tTwist; | ||
| return true; | ||
| } | ||
| } |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Oops, something went wrong.
Add this suggestion to a batch that can be applied as a single commit.
This suggestion is invalid because no changes were made to the code.
Suggestions cannot be applied while the pull request is closed.
Suggestions cannot be applied while viewing a subset of changes.
Only one suggestion per line can be applied in a batch.
Add this suggestion to a batch that can be applied as a single commit.
Applying suggestions on deleted lines is not supported.
You must change the existing code in this line in order to create a valid suggestion.
Outdated suggestions cannot be applied.
This suggestion has been applied or marked resolved.
Suggestions cannot be applied from pending reviews.
Suggestions cannot be applied on multi-line comments.
Suggestions cannot be applied while the pull request is queued to merge.
Suggestion cannot be applied right now. Please check back later.
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
You should be checking the actual data of the laser scan.
Grab an actual simulated laser scan from V-REP..
Write its contents to an e.g., textfile (or just dump them in the cpp file for now. Then in this test function read them back, convert them to a CObservation2DRangeScan and check that the data of the latter object is still there as it should be...
This may require some tinkering with capturing the data and reading /writing that text file but it is the most definitive way moving forward to make sure that the convert method produces the expected results...
Refer to this as an example: https://github.com/MRPT/mrpt/blob/master/samples/slam_icp_simple_example/test.cpp
Let me know if you need a more detailed explanation on this