diff --git a/.gitignore b/.gitignore new file mode 100644 index 0000000..2f2db5a --- /dev/null +++ b/.gitignore @@ -0,0 +1,7 @@ +.vscode + +build +install +devel +log +lcov \ No newline at end of file diff --git a/CMakeLists.txt b/CMakeLists.txt index 1071303..a0e2a76 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -12,17 +12,26 @@ endif() # Find dependencies find_package(ament_cmake REQUIRED) +find_package(Eigen3 REQUIRED) +find_package(rclcpp REQUIRED) +find_package(sensor_msgs REQUIRED) +find_package(pcl_conversions REQUIRED) +find_package(PCL REQUIRED) # Specify the include directories include_directories(include) -# Create executable -add_executable(${PROJECT_NAME}_main - src/main.cpp - src/api/m3CommandGenerator.cpp - src/api/m3ResponseParser.cpp +add_executable(M3Publisher + src/imb/ImbFormat.cpp + src/imb/M3Listener.cpp + src/imb/M3Publisher.cpp ) -ament_target_dependencies(${PROJECT_NAME}_main + +ament_target_dependencies(M3Publisher + pcl_conversions + Eigen3 + rclcpp + sensor_msgs ) # Install the headers @@ -31,28 +40,42 @@ install( DESTINATION include ) +install( + DIRECTORY test/data + DESTINATION data +) + # Install the library install(TARGETS - ${PROJECT_NAME}_main + M3Publisher DESTINATION lib/${PROJECT_NAME} ) +target_link_libraries(M3Publisher + pcl_common + ${PCL_LIBRARIES} +) if(BUILD_TESTING) find_package(ament_cmake_gtest REQUIRED) ament_add_gtest(${PROJECT_NAME}_test - test/main.cpp - test/XmlCommandGeneratorTest.cpp - test/XmlResponseParserTest.cpp - - src/api/m3CommandGenerator.cpp - src/api/m3ResponseParser.cpp + test/ImbFormatTest.cpp + src/imb/ImbFormat.cpp + ) target_include_directories(${PROJECT_NAME}_test PUBLIC $ $ ) ament_target_dependencies(${PROJECT_NAME}_test + pcl_conversions + Eigen3 + rclcpp + sensor_msgs ) + target_link_libraries(${PROJECT_NAME}_test + pcl_common + ${PCL_LIBRARIES} +) endif() diff --git a/README.md b/README.md index fd431f2..2bd7f6f 100644 --- a/README.md +++ b/README.md @@ -1,2 +1,10 @@ # vortex-m3-sonar-driver A driver for the Kongsberg M3 Sonar + +## Specifications +Requires an onboard CPU running Ubunto 22.04 and a CPU running Windows for the M3 API. + +1. Start the M3 API on Windows, and connect to the sonar head. +2. Edit the parameter file to match the Windows API's ip address and port (defaults to 20001 / 21001). Also choose what ROS2 topic to publish the pointcloud to. The callback_time is given in milliseconds, and decides how often the software will try to get a new image from the API. +3. Run the M3Publisher file on Ubuntu 22.04 using ROS2. + diff --git a/data/utf8.txt b/data/utf8.txt new file mode 100644 index 0000000..81353c1 --- /dev/null +++ b/data/utf8.txt @@ -0,0 +1,207 @@ +;����+�Ă2�P� +˻��dž�;���;�]�*�|� + �(/կ�:�/���T�� +%��#}�/��509߮#(0��a�u�ð���'�U��>x�0�ƿ����00�M�0o�O/s�0�a08t0�@0��02�>�� + (��)�}����/������/��0������/��w�N�0�ԅ�n>e��.$f0�Ѥ-�*0#>��ّ0�n��#g�� + �0k��0.{E/kT�/��.�Rl_⬰����0KV0�G�/��/q��0E9���&0��Ѕ�{C��|�.X�g>0��37T0W&��`0�[/��0����6�.��y�:�0x!)0W��I���[!���������N�\�,���>2,0��P��� +���&�0'm��J��0�H��fj/���R�.�@�_/�J[�Y��ϰ�0i�<��"�o��0:�7i�/ ���BO90����%�/T?��R�0uN����� +�/�9ï2I��J�/�`����/eY{0�<��c-/v��.'�)�y�0��l/aO��Gm0��0m�Y/�ho�"�d�$�W0�G03$�/�c���x���0���/ާ�0�^��OM/1�07II0^��/�y�0�����~��0���&�̮ + ��u0q�0�oX/� +������A�0,p��� +���*0d��bѺ��)�0eg��E��M�U0{�����/�*����0.0tF�/�_/�o�-�=l����/߬_��/�N���K/��c��4.B� 0&00>90�l�� + T���3�����c-k��`0��1�.�"0���/*Y�����Q�u��\�/�u�-���/D�%��5ׯ7H������������ + ���&�G0�'��{��0R�H�f�0�/|��Ns��9V/�� +/��0���� + !m�� + �0���� +X.�_l0�t�.^l0��^�/���b��pB�/H�v/��]0m�0�s�0MxF�")�+�3/R0���eW�����b�/�0G��0�IC/�^/0/�0|�ޮ�T04+믮�/-�/���+f�q/,�0$S�ڽ��77�0&N�0��{�0���0�6�00 +n�#!%��0����[��I�/Z���]6����0�w00�c�cEm/,)�/���a(;���BUQ�.s}0�ȉ��̯�H�-�� 0 �1���Э���/�I0�M��Rq�+_B/Iy���Q���/@Ś0�0�0]�0���-e+/�a�/pIد@� + ��/a��"�S��L����,�8ţ���h/>�/���D׿����/ ���禸/��w����/n���>�0�r + �� @0���/,.N0 ��0���������z/�]�0���D�-���XYR��I0!�0>j/CT0-�}0#��0]��0����0"�-"!j���`��{ܯ"D�0�ʨ�� �-��0��.--�/�x0g��/����:�����e + Ͱd + 2��ˉ2��3��@q0�ݰ��0��/�/�~R�w���>�1�HW0>���0&/0�e���*/��/�k�/��)�0P���t0E�����;/j*Ȱ��,��V�.���n8䰆���}a��0)D-.�ɰ0WxΰX,��׸�7/1Bw0��1�,s0�6���n��d2�Ʈ�� ˗�!��/�NV0$���I�0�Eٰ��U/���/�ڂ��/�tɰ@6V-BlB��$0cS��0�C/��$����nT�/�J�Y^�0�e��p�a��|*/L�/5�/��0 +����ܯy�Я�y���6�/뭓0�F.(�0��!��l�/]����� /o�s0= + �/Z���OI0"�&�d��0%�|�I�߯l%I�~�/�ˈ/�^)/��o8q���.Ӊ����0T��.�����+�/H.��/����U�믖�#0e%J�{^����ɰ;us/��Xk0%pg0�d�/7��0�*�/᭼0��/Q}�3�����?/c��/wI�/j�"0��G�#�����/�Ә�L��/ + ��a)�/������ӮI��k��/��4����.��دY��.|q�2:'/��p.ˡ����ޯJ�J0�cj�2��0Fj�0�Ұ�x3�"l�0�z�02871#�L0�T԰�@1��0��ȯ�5A���1[�+�� �0��ݰ>~B������C0���D0�1հ�i�0�f1������:0��`/��/��`�u�[� :�/<��/�Ɂ0h*b���0Ԥ:��u���� + 0O��e�/Y��KV�/,+0�I + �P��mO50�꯰�+�.z%�.v׷/0z&0[�'�Me���V0K_�.�S{�(Ok01�\��)(0��P0 + l�0Tw70P��/���.l0���*�)0:Z����.!����0 ѯ + �.��/(/S�Ͱu%�}BT��07T�0��4".�,P��U'1��Ӱ0uR0�/�an0s��/ܢ�/�/�� 0�����ͮ�0�]�0�P��H�"0Ғ���2 + 0��|0R��0֡ + �lzM����ϯc.�k�/�ǜ/O-�0oɈ��AA/8���*0�V�� +b0F�0:l�.�y����/�j/�U/f��wB�0D�805t�0?�w0,��/!ׇ�|�Q�j.0�ۙ0�y���/�7$/ʸ�� +T!���+.sBN�3Ǽ���/\����Ѱ�=�0��8�A;1�'�� + k0;��0X + �0�t˯E���j\�%�0�\���.���Dg/|j®n�k0i]K�/�_��nZ��E�?�}T��'�� +��l#^.킠��͙0f�0�� 0���� +����0���wv���h/��0�sh/]a���ΰR�/��-*T���T�09*��따*�;0>G.�W0ô`.C���\�ѯ|��'�/� 0:�����\/v��9�-O$ + �*��/�F�/��G�3���Cg��Mp�!r�0>X�0l��/j����#�0�80'����M0�/G`���20�i�0�p���ܰ�f/�������m(.ѷ9�4��/��*14-��R��0��J�/lG0xf����0Ұ��.g;��.�pch0��D���00n)f����0;���W3����,�ɔ�0� +�Z��/� +�d0I�g��G�0.���v�]0F���X8,y��064������/����h����IP0����D���0<�{�Ppl0B����bu/9��%@H�%�#��%0����+��5^�0�?0zP���/mh���/�{v0�o��kuX0�/W`��� + � + �-��b�;ï + Qذ'O�0��17 + � -�0R7ޮ&��.x��0�گ0��0��/��C�/��R/���1����0�t��T!�ެ��P�1H߀��� + 160Ue0��0Y?�-���0���/����� + ���40Xh,��� 1'�10�F0��0F�80R��/c�/�Z�0&T���/?�:.�%��]�����uc�-C0ݰ�ȋ/��/}�����/>�̯�� +0N��/ҽ0M�0��/Ÿ�/N��.�����?�w'�/}�K�����B��/��ஆ740X����/�0�QJ0��.0��-��a�Ӗ +0��/`���;��/[�߯zs0��.0��U0tP0ށ�0�ȇ0�j"-'�c0��n�,�Q/�C�����o�����B���0�����hg0�g/��.t��WCE��I��.�/��,0/t��.܁0���0)0N���0��=�#؏�^f6��9�~҈�fv��i�U%;� + 1�L�� + Y�I�#1?-11$�䰊�%��;��M�0�`@0�o�N=T0�g�0t�Y[/�Q^���<0ݫ0k�b1M��0�l�X�c0%�A�d.�00tw�/#雯�(Q���0/�=���/b�08d0��G�(c{�u%0�̏�!�J0���������B_��Yϯ�e03TS/�}0Pgծг4���F.�n���q������A�K�0$��/Ȓ��m�_�a�0F�0^`W0(z���ZE0�6��c�/P<�0H/i��0i;���~�0?������/3T���f�/K"����s��87��������.�F��rF0L�-/�#\�"ˏ���0�/t�R0�֮0�^�0��>����폰� /B�/�Ӄ/ntq,rWذr0�3A�/�K-/`l�/���/ór0-����09/�Ւ�/�VC�`K�� +0_��0�����aE�Ur�����/#�s��Y�����IܯRo*��g��ǯr��0�Hh�07���R����/tY���B0�����+�0�[w0�Tԯ����D��0������dš�E�R�;�0!�]�@.41����+��H03��w@���鸰20G�)�Fu�[��0Zg��'��TR,�ݷ����,0���ǿ�0��T����@*F���/��0�w����K/���%0,����0�h�0��/<����)'��[��v҈�aՀ/njK��\'��G^/��.}�L/Z���ݺ0��^/b0�j!���0ZB����0�_)�,x�0X�����W.�=f0D���y/��6��]��*�Y���E(q0 �[��ͨ��^+� ˥��?�[���\/�"�/[+�� z�0�5��I�Y0+�S �W�\0�0�܅0^��-B�x��՜����a����.���0�1~��0W�%0��R0�/O�P/�/�0�h�/��/T)0ح6� �`0���+Tg��Ğ.Ch2� + ��Y��7�0��o��͐�7�0�i�N}�0�x1���/�l�0,�/�<0�R��AS0�0ZJc0xn"�& + /�J�0�0G�s0�Q0W)�u(/�}��d߰���0xX��'yC0�7�.0q���2��/�Z�. +��.�x0�I +��6]0fT0�Nf�����;���pY/}�įok�/:̨��R�0��%.f�����\/򻱯�.o0���0z�ί�7¯y�0��0�C!�ф�0�S֯ �P��.-���������ɰE#���M20y\�/���0o���\c0��A0��L/Q���~��/�e�/��//̠��Qq�0Џ��X��wjv0�}�0�M<����M<��� +2Y�c���IjI0�E6�����>%�0���-κ����q� +�/)��/���0*�.�+0�b�0��0pa�0�N3��X���'�-5z �~�M����$��/�Z�/�:��I&0 @/����1Y/�]2�2�V.�dJ0��ů�,��.����11a���4����_i��1&0"Vװ��/�ڢ�Gb��� // + 0�$q��1��e0�� + 0;�C�>s�.�R-0�T�0�|Է�� ��~Է�� ���0��z��M�#��,u�� + < +v�:� +�}o�.U�����#b�fְ=�(1/��0�O-.�z(�6�ٰʹV0����L�0髄0��E��,��/�4�p.��/V��.��g�A�c0�$"�*X���m����Uz���p�;Sz���p�;i��0��B���U0y���@��.��U��e�/G���H��/���/�g��# + =����M;�0��f�50�Z�b~�0�r8�� ���/�z0�3�/�09��0�,1���0���)�6�� + F60�-�/�1��ݰח&����0�착!񰺗l�Oo�.@��;�Ŧ�A��;�Ŧ�)ٵ���&�߭�&尌��0I����c;�,��c;�,��؂�����P�;0t�0�DͰo��.p(گ���/=��q8���ϻ\e����ϻ{e��B�̰�j0m8�U5O0"�/�tH�-�0!�q0� 1ܣ�/�� 0�z0 + �0�4���<0�p���G��A��*�I0ݯ�J�V�/�~ᑯ +��}P��3��K + <��Y�һ˯߻�a���'̻a+Q���d��ꕻ1�n:<�<�];�n�<��F�R��<��5�����oD���#<%��;��#<'��;�����a��Ԕ;��b;ˣX<�@;ěx<2������;�����X_9��f<�K]����0g�������0T�诵 + �^��/�2{0�x����c0�"J�۪=����}��L����V���=�qι0�A��B����/ˇ�I�9��}��`~-�#d/|�0��20�K�0Mh�-��ԯ��ʰɤd�n嗰\�W.�;��D/��k0��/�3�0(� 0��.r��0�Br��[:/����S�0/ )/0 ��w�������K.��/���0&ň����0��w0/�P����0�,�-�`�.5�0��Ȱ�j�0�� +0FР�D�0��~�o�0Z���帉/��0�ڰ��0*yI/��'.�:�0�2�/�Ưm0dS�0�@0���/%�?0��¯Z�X��9��Beİ��L0�/����0l��/pTX/Z�*� *�0�?��E�/�W�҃���0?S80���.��>0�+�/��*�L�w0ۛ�.˱0�,01�`0(��/K�.9��06P�0��0�2W0 +���]�z0,��0c�e���/�X0��|/�7'1�#����0YGj��.�0�A���6,/6��/�3�0��_�w�0 + ����������L�.3�G/�C��a.���/]�j��3_0��ۯ2���0� 0��c�@I���DZ�i�E���ðnt8��������.j`K��i0c�_�H���/<�0@c{/5Xv.,M>051<.yai/k��0�"�.c�!1,+500}�.2�0����L���@�0���.�M���L&��v�k��/;^��Bb��j���I30N6��`��/S�/��d08/�5�(V� + ���'��0kl�e�E0n5 +�0��x�Y��0B� 0��+0�����L�/�1R���/0d����v_.:���X0rO���{�;9�� +t-��(���@)0Z����0�Z0Q�0z�O0N�0kz��&� + 0�k�/;l�/���0��E�ಯ��/x� + 0��0*�Я����ΰ�{���gg��$ӯz�d��T��>ȯև˯yq)0�������!GF��9�0� +�ͫ���_t��3�!�}r�/u���(�0X���+���� +�2#��X0�Tt�Np��3�g/<}���z0���/�k��rE.OL��&���\0n�0��0��/��"�s���� + �/៭��ߋ/iP���I�/̜�/�"��6�;0�涰x�R�B�W�p�° +��r�-0 ʌ0�Ѩ�6�m0�=c0���0{�A/�qb0�`0�b$0�ԡ0!��/���/�~˯pc���D��ܳ/ +6������/ + ��U�0����͖�6�k.P�E0�2���70ા/2�\0#w��Wկ7�/��00�v/]'�.z�/�B[�����¥/�im �� + ��џ�)��-ag� +�1&��7�<.� ��u��E0��}���0�ʏ0�!ǯ\�0�P0}ɜ0|:�0݄/�g�/��04� ��AC0��r�+�-N +�W1��׊�/j��/��!0��08a���#0�x�� +��!����؎.�Qu��ș/�`o/a$�0���.Pi�/�0h��/h�vCq/!�H���0=�z0h�~���1�YR��=�Q�U�.�����,�/�6���/0�3/����qN0nǯSV4�{_�a�Aȋ����0p=����0Җ�����좯^�4�:�?//���o���?�0�ɭf�Ư�]�0~��/oo�/}� ����0�����/Ӣ��"0t + ��ʯ�q�/�=��?����/bl�܀0��l0��T�N�ޔe�W�/�W�0<��/�9K.��.0��0V�0�J'. +oR�u^|0�d�0��/�������-�͏��0����0Ҹ���1LB'0q݇Q0�L�0��$`���p���~����ՠ/ + �/<,0�0Iّ�E�`�^6��#}0�)��U'�q��/���<��z��0v� � +�k%<0ɻ��M.1�����0��0 + �a/b�7/�3�/>V��{0��p�$/z�g��Ε.:�I������u�/;� + 0) +Z0" ~��*5�ڢP�1Е��J��I�d��q'�|�.,0V�0��O�鯧��� ZI0�^ + ��/ ��I���E�t/幭��H.+Yu/�Vz�n�诋�'0����~(�0��)��)@0,r�kbQ-�,���q��/"8�0�B�Ѵ����09ػ�'��ͽ�0�%J0h3��?�K�����^0�>���WN0ܷ5/�É/���0=A0]�0���.�Ј��0�w�rk.���.��l0���-�\�-P�./���-����: + 0���.V�^0Or00�1;�/a����U*���)0䁀�f!���2�t��/m]0$l�0�g�.��i/Ō�0�Թ���00�} +���v. B�.�ޮ��5/笣0D�S�3�.b�0�y�0�Q��"�0���0Na�0��1����|��1�/H��0��s0 +]A.��g�b>N�ζ[���{/LL��8'� + �/���/�V��K��6�j�(�0;�m��.30�E���H�0j_ + 1]#�/@c�0����ZJ��"0��%�@Zӯ�Q����%/$a�&˪/������W�qc��8�6� +�O:0},��4q�U�k�x< + 0xK+04b�0^E0�z + �3���V�/�y/���0 + ����$�0�� 0@DW0��02䰹w$0�Ǟ0*�;0Z�h��Yǯ>����?��-��4��0�@j0����������.�',�d,�����\Ǯ�͝0�α�żp���/`8�/ef\�{݆����,�0�hQ�����<�/�_��fP�'[_/'�060'Z�/݂0��ү)��/��/�1��X=z�u�/���گ�����v�M���d�D{0r:=/R�- +�/j�///帰��c���)�0s'0�1��ӯ{C�f耰�!�������/�"�/f=0����W/�2�� 0����}�.��!0�j0~���-(0=����/e��0�j0�+���_�h�;��y�-?�"0SUT.�0� ���ԯ���B���0�[��1( +(�ZR_���w0$F%0c^�+�/4��0���0�l�0z=i���0�ٮ��n���R�*/��*�0\����[���y0�vx֓�$_�솜/,V:gԯ��80��,&|��Uz0u��0l�0qg�/�I��D<>��n/|ދ0 +1<@0�H��%4/���d���6/+� +�Y�l�G�%�%T�fJL0��˯������N0��I�Bl1�<`/�h0�0��A���0��������8-��/�d��~!�0N���w.���.�3<0�fگ��d���pSO�r)��c���R�*��0�쯯��ۯ�@���=����V0pL ��X�0ͱͰ��ð�N(0V� 1`�40�"1��60`S��P.ïz�M0+Z��r��G�Ͱ�F�5p�/ +�v����/)���Kp�]�0�f/0rd����u����lV/YD>01��zBY0͕�.!��/��]��* M��/�JK0�a�/�1�vP�000<��/5�گ���/�L��#b�0�r�W��0��0$40��^/*]��vl� + /�0xÀ��0��d� � + � U03t����.{:���Ԍ���p�@>/�%F0_�L�ۮ +�H�/:(ή_����-�˷�/&疰�n0�n/Bذc/;kN/��/�����i0j�8�63*��Ѱx3�/ߜ���/It�0,��/"��05��/ﱮ`9I����0��v0�S10R�㯷�T0A�O0S��� +�0]�e/� �X0i�Z0�ok��ߺ�*���f�,/V,��F�!������!��Ҭ0�˂��`8�T�/c�/��H�4Ӿ0�^��=f�s���P�0=X������l0ӭJ���������0�ܯ�i:/�1�ְ�|�0�����0�ɚ-�C�0�����/�����`0&d�1��.X=K��+�/_vh�fZ}0���0"����0������@�1 �=05�?��0�2�0�f��v�(���΃���l)0 +u|�e�0���/������t05��0#(�N��-���/h��0�O�0��f1��0��n�(��0�}C�1p�0���2I\�6�!/3 ����zt 0�nl�N�/Hr�/+���P?0�[��%0G�3�:0��߯�0����/�ΰ +W��������,��ů)0��-Ŧ�0ϊƯ�������E�90)���N/q=�0�x�0�ӯ���/�y�/p.��t�/� + ���P�R�g���c��o���W�����0~*�/����&*��u!0ї|0��K0�0��t0��Z��)6//+�0� /g�0$k����0���v�0��P-0u��e�#�׷��Vb_/p�����5�!�=0���-@�r��`���/�5�.jo@0,ɜ0�B�0��[�Щ�z����Ҟ/�Lb�|��/I�-U�ذ�ͮs�A/=8V��<5/欧/#�_0�W����0�~�/��.������;���20 + ��0.�,/,�/��1�&o�/�����İ1w/$��>2��d�/��� + E�0{��?�kH���,02ċ��Y0 + ����0�Q00��ү�a + �_V�0� + ۯ�a�����g��-Lx0��� K.�^���b�'0^����گ��k���Wv0˯�bd�ë�0�8찔����1���K��f0"g�����0s���q����^�tT��q\�0�$����*0�ׯl�20�P.5"�.�̩0X����d��l�b�į��� + ��s+%�l�W����/w#1/�.���t��0�"/�0.>�o�0(��Ǧ07<�O-�0�,��SO}���50�2���q_/�g_�]�A�lu��go��z0U�ů�W°�%i�tB񮧔���N,.�K�/�� +0�M�����0��~�j� 0������90׌C0ݟ�0dz��EZ.-ꂰl-�x�.d-�)i�0��0|%�0�!40�"j0���/��o/��0O1v��/��0��/��M0��ɯ��E0C��/�j~0Ԯ}0�C���th���O2�m��f�/�������0�� + +�!��P�v�_�۰�w�0ne���C0�] + k0����0Eh�/��ۮr�0jX�=U0T�D0�5�Pr*��g���h�?7�)�00��� + (a0�BF/(���_VU����<�O0L��/3�+i�1~�0G�/�Y��L�v0`�.�%=�Q��&��Z���ﰵ �i�;.w/+M�0����H0�@L0_�30�'ٯ>2f/u�/D�.�s���0�HE�9�į��$0���0�� < + �2��� <��2�����Ŝ���9p.�P�rй��a{�J�K���k�X0������N��0�0����S����iM�V��/��0�<�0 ��/��O05Z�0r��/�"0դ���k��H�/k=#�E�M�TƵ�r)߯&��71�sf0 ��0m����|;��ɋ���ۯ���.'O/�a0].4��߯�"��`0*1�<ӯ)���h�j�>4 + 0ŷ����r�� ������ �01[g���0�=0�0�Ꭾ�e/��Q0\�l0���/�Nl�@G%����O6f��o�.6��0�F'����0�_3�|����60�+ܰ�$V��0n�/2O�/A����C�0b��0Y]T0�3�._�?� + ����e/P�<����B�/�W�����<0Qe�����*mӯ�HB�bX6��]�;_X6��]�;�.�05���'0�.�������������/t���e0/�`/ aN� + �F�ai�- + $�a����Vc0~�Y�I0�0�Y��-�/^B0��0�SL/�O�0��0�^�0V��0!�������G�4�/��R0��&1����Of.{`k0u��,ذ!�Ѱ5�����;Xp����;^p����/����&/ + ��C5�0=۴/1��8N+��,��8N+������Mj�=�00���0��ǰ��/��8���1-��@�d��-�չ���:��չ���:��4���s�/ +Q�E�L�r�0�T0�ު0q�����"/UW�0ݳ�/�c0�u�0�ү�10��?C��G�H�C0o��]�6%��9���V�H/!w�0՞�� + ����{�J߳�䣘/]��/B���Y���S;$����S;'���/��}���/��<}V:v�0�0��0<��/r)�/��d0� +;Ю�9��9�62����R������غ�m*����;�m*����;�b?0��v��v�/[��/�wJ.�/��e�"<+�ӻ�y;:�c�ٔȻG��IRi��03���C���L�:u��;�L�:w��;1<�8�91Ĺ;P��;� �;o;�;� �;o;�;�b.���0�7�;Hl���7�;Gl���&$�n�:I\��*l�;֑��M(;�ѿ;��w��ѿ;��w��7��Gl�;�=������_&�A�'����%�;��m��!<0!�B؅;W���:�ջ�8&#?�ч<(�;-��*�;.���� <�" +#include +#include +#include +#include +#include +#include + +namespace m3 { +namespace imb { + +// Available data types for the IMB Beamformed Data Format +enum class DataType : uint16_t { + FloatComplex = 0x1002, + IntegerMagnitude = 0x2003 +}; + +#pragma pack(push, 1) // Disable padding for the following structures + +// Structure for MUM_TVG_PARAMS +struct MUM_TVG_PARAMS { + uint16_t factorA; // Factor A, the spreading coefficient + uint16_t factorB; // Factor B, the absorption coefficient in dB/km + float factorC; // Factor C, the TVG curve offset in dB + float factorL; // Factor L, the maximum gain limit in dB}; +}; + +// Structure for OFFSETS +struct OFFSETS +{ + float xOffset; // Translational offset in X axis + float yOffset; // Translational offset in Y axis + float zOffset; // Translational offset in Z axis + float xRotOffset; // Rotational offset about X axis in degrees (Pitch offset) + float yRotOffset; // Rotational offset about Y axis in degrees (Roll offset) + float zRotOffset; // Rotational offset about Z axis in degrees (Yaw offset) + uint32_t dwMounting; // Mounting orientation +}; + +// Structure for ROTATOR_OFFSETS +struct ROTATOR_OFFSETS +{ + float fOffsetA; // Rotator offset A in meters + float fOffsetB; // Rotator offset B in meters + float fOffsetR; // Rotator offset R in meters + float fAngle; // Rotator angle in degrees +}; + +// Structure for REAL_TIME_SOUND_SPEED +struct REAL_TIME_SOUND_SPEED +{ + float fRaw; // Raw sound speed in m/s directly from sensor readings + float fFiltered; // Filtered sound speed in m/s after a median filter on the raw values + float fApplied; // Applied sound speed in m/s after filtering and thresholding + float fThreshold; // User predefined threshold in m/s used to get the applied value +}; + +// Structure for PROFILING_DEPTH_TRACKING +struct PROFILING_DEPTH_TRACKING +{ + int8_t nRangeDeepLimitPercentage; // Percentage of the maximum range for auto depth algorithm to switch to the next higher range + int8_t nRangeShallowLimitPercentage; // Percentage of the maximum range for auto depth algorithm to switch to the next lower range + int8_t nMinPingRateInHz; // Minimum ping rate in Hz required by the system + int8_t nReserved[5]; // Reserved field to keep the structure aligned +}; + +// Structure for GPS_QUALITY_PARAS +struct GPS_QUALITY_PARAS +{ + int16_t wQualIndicator; // GPS quality indicator + int16_t wNSat; // Number of Satellites in use + float fHorizDilution; // Horizontal dilution of precision + int16_t wPosFixQualityCm; // Position fixed quality in cm + int16_t wReserved; // Reserved field to keep the structure aligned +}; + +// Structure for Packet Header +struct PacketHeader +{ + uint16_t synchronizationWord[4]; // Synchronization word, always 0x8000 + DataType dataType; // Data type + uint16_t reservedField; // Reserved field + uint32_t reservedBytes[10]; // 40 reserved bytes + uint32_t packet_body_size; // Packet body size + + /// @brief Default constructor for PacketHeader. Read the raw bytes from memory + /// @param byte_array Start address in the memory + PacketHeader(const uint8_t* byte_array); +}; + +// Structure for Data Header +struct DataHeader +{ + uint32_t dwBeamformed_Data_Version; // Version of this header + uint32_t dwSonarID; // Sonar identification + uint32_t dwSonarInfo[8]; // Sonar information such as serial number, power up configurations + uint32_t dwTimeSec; // Time stamp of current ping in seconds elapsed since 1970 + uint32_t dwTimeMillisec; // Milliseconds part of current ping + float fSoundSpeed; // Speed of sound in m/s for image processing + uint32_t nNumImageSample; // Total number of image samples + float fNearRangeMeter; // Minimum mode range in meters + float fFarRangeMeter; // Maximum mode range in meters + float fSWST; // Sampling Window Start Time in seconds + float fSWL; // Sampling Window Length in seconds + uint16_t nNumBeams; // Total number of beams + uint16_t wProcessingType; // Processing type (e.g., standard, EIQ, Profiling, etc.) + float fBeamList[1024]; // List of angles for all beams + float fImageSampleInterval; // Image sample interval in seconds + uint16_t nImageDestination; // Image designation, 0: main image window, n: zoom image window n, n<=4 + uint8_t bTXPulseType; // Tx pulse type (e.g., CW, FM up sweep, FM down sweep) + uint8_t bAzimuthProcessing; // Azimuth processing (0: off, 1: on) + uint32_t dwModeID; // Unique mode ID, application ID + int32_t nNumHybridPRIs; // Number of PRIs in a hybrid mode + int32_t nHybridIndex; // Hybrid mode index + uint16_t nPhaseSeqLength; // Spatial phase sequence length + uint16_t nPhaseSeqIndex; // Spatial phase sequence length index + uint16_t nNumImages; // Number of sub-images for one PRI + uint16_t nSubImageIndex; // Sub-image index + uint32_t dwFrequency; // Sonar frequency in Hz + uint32_t dwPulseLength; // Transmit pulse length in microseconds + uint32_t dwPingNumber; // Ping counter + float fRxFilterBW; // RX filter bandwidth in Hz + float fRxNominalResolution; // RX nominal resolution in meters + float fPulseRepFreq; // Pulse Repetition Frequency in Hz + char strAppName[128]; // Application name + char strTXPulseName[64]; // TX pulse name + MUM_TVG_PARAMS sTVGParameters; // TVG Parameters + float fCompassHeading; // Heading of current ping in decimal degrees + float fMagneticVariation; // Magnetic variation in decimal degrees + float fPitch; // Pitch in decimal degrees + float fRoll; // Roll in decimal degrees + float fDepth; // Depth in meters + float fTemperature; // Temperature in degrees Celsius + OFFSETS sOffsets; // Deployment configuration including translational and rotational offsets + double dbLatitude; // Latitude of current ping in decimal degrees + double dbLongitude; // Longitude of current ping in decimal degrees + float fTXWST; // TX Window Start Time in seconds + uint8_t bHeadSensorVersion; // Head Sensor Version + uint8_t HeadHWStatus; // Head hardware status + uint8_t bSoundSpeedSource; // Source of the sound speed + uint8_t bTimeSyncMode; // Timer synchronization mode + float fInternalSensorHeading; // Heading from internal sensor in decimal degrees + float fInternalSensorPitch; // Pitch from internal sensor in decimal degrees + float fInternalSensorRoll; // Roll from internal sensor in decimal degrees + ROTATOR_OFFSETS sAxesRotatorOffsets[3]; // Rotator offsets + uint16_t nStartElement; // Start element of the sub array + uint16_t nEndElement; // End element of the sub array + char strCustomText[32]; // Custom text field + char strROVText[32]; // Custom text field 2 + float fLocalTimeOffset; // Local time zone offset in decimal hours + float fVesselSOG; // Vessel Speed Over Ground in knots + float fHeave; // Heave in meters + float fPRIMinRange; // Minimum PRI range in meters + float fPRIMaxRange; // Maximum PRI range in meters + float fAltitude; // Altitude in decimal meters + float fHeightGeoidAbvEllipsoid; // Ellipsoid height in meters + REAL_TIME_SOUND_SPEED sRealTimeSoundSpeed; // Real-time sound speed + PROFILING_DEPTH_TRACKING sProfilingDepthTracking; // Profiling depth tracking + GPS_QUALITY_PARAS sGPSQualityParas; // GPS quality parameters + uint32_t b3DScan; // 3D Scan with rotator + uint32_t b3DScanAxis; // 3D Scan rotation axis + uint8_t bSonarClassID; // Sonar type used to collect the data + uint8_t byTX_Enabled; // For Forward Looking Sonar multiple transducers + uint8_t bReserved[2]; // Reserved field + float fROV_Altitude; // ROV Altitude + float fConductivity; // Conductivity + uint8_t reserved[3796]; // reserved field + + /// @brief Default constructor for DataHeader. Read the raw bytes from memory + /// @param byte_array Start address in the memory + DataHeader(const uint8_t* byte_array); +}; + + +// Structure for Data Body +struct DataBody +{ + DataType dataType; + size_t size; + Eigen::Matrix magnitudeData; // For 8-bit integer magnitude data + Eigen::Matrix, Eigen::Dynamic, Eigen::Dynamic> complexData; // For complex data + + /** + * @brief Construct a new Data Body object from the raw bytes of the TCP packet + * + * @param byte_array Raw bytes of the TCP packet + * @param nNumBeams Number of beams + * @param nNumSamples Number of samples in each beam + * @param type Data type (FloatComplex or IntegerMagnitude) + * @throw std::runtime_error if the data type is not supported + */ + DataBody(const uint8_t* byte_array, uint16_t nNumBeams, uint16_t nNumSamples, DataType type); +}; + +// Structure for Packet Footer +struct PacketFooter +{ + uint32_t packet_body_size; // Packet body size (4 bytes) + uint32_t reserved_bytes[10]; // 40 reserved bytes (10 x 4 bytes) + + /// @brief Default constructor for PacketFooter + /// @param byte_array Address of the first byte of the packet footer + PacketFooter(const uint8_t* byte_array); +}; + +#pragma pack(pop) // Stop disabling padding + + + +// Define the combined struct of the IMB Beamformed Data Format +struct ImbPacketStructure +{ + PacketHeader packet_header; + DataHeader data_header; + DataBody dataBody; + PacketFooter packet_footer; + + mutable bool validated = true; + mutable sensor_msgs::msg::PointCloud2 message; + + + /// @brief Creates the complete IMB packet structure from the raw bytes of the TCP packet collection + /// @param byte_array Start of the packet + ImbPacketStructure(const uint8_t* byte_array); + + /// @brief Validates the contents of the packet + void Validate() const; + + /// @brief Generates a pointcloud from the data + void GeneratePointCloud() const; + +}; + + + + +} // namespace imb +} // namespace m3 diff --git a/include/imb/M3Listener.hpp b/include/imb/M3Listener.hpp new file mode 100644 index 0000000..2ac4615 --- /dev/null +++ b/include/imb/M3Listener.hpp @@ -0,0 +1,48 @@ +#pragma once +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace m3{ +class M3Listener { +public: + const std::string addr_; + const uint16_t port_; + std::vector& shared_vector_; + bool& packet_ready_; + std::mutex& mutex_; + uint8_t buffer_[1024 * 64]; // Shared data buffer from the publisher + sockaddr_in server_addr_; + int client_socket_; + + /// @brief Default constructor + /// @param addr Address to the M3 API + /// @param port Port to the M3 API (default 20001 / 21001) + /// @param shared_vector Vector to write the data to + /// @param mutex Shared lock + /// @param new_packet Boolean to indicate if a new packet is ready + M3Listener(std::string addr, uint16_t port, std::vector& shared_vector, std::mutex& mutex, bool& new_header); + + /// @brief Creates the socket with the given address and port + void create_socket(); + + /// @brief Initiates the connection to the M3 API + void connect_to_sonar(); + + /// @brief Starts listening for data from the M3 API + void run_listener(); + + /// @brief Closes the socket + void stop_listener(); + + /// @brief Destructor closing the socket + ~M3Listener(); +}; +} diff --git a/include/imb/M3Publisher.hpp b/include/imb/M3Publisher.hpp new file mode 100644 index 0000000..95db5d8 --- /dev/null +++ b/include/imb/M3Publisher.hpp @@ -0,0 +1,40 @@ +#pragma once +#include +#include +#include +#include +#include +#include + + +namespace m3{ + class M3Publisher : public rclcpp::Node + { + private: + /* data */ + std::vector data_vector_; + mutable std::mutex mutex_; + rclcpp::TimerBase::SharedPtr timer_; + rclcpp::Publisher::SharedPtr publisher_; + bool packet_ready_; + + /// @brief Publishes the message to the initiated topic + /// @param message Message to publish (PointCloud2) + void PublishMessage(sensor_msgs::msg::PointCloud2 message); + // M3Listener& listener_; + public: + /// @brief Default constructor + M3Publisher(); + + /// @brief Processes the data from the listener + void ProcessData(); + + /// @brief Creates the listener + /// @param addr Address to the M3 API + /// @param port Port to the M3 API (default 20001 / 21001) + void CreateListener(std::string addr, uint16_t port); + + /// @brief Default destructor + ~M3Publisher() = default; + }; +} \ No newline at end of file diff --git a/m3_params.yaml b/m3_params.yaml new file mode 100644 index 0000000..0b00c36 --- /dev/null +++ b/m3_params.yaml @@ -0,0 +1,8 @@ +M3Publisher: + ros__parameters: + sonar_api_ip_addr: "10.0.0.153" + port: 20001 + topic: "m3/points" + callback_time: 1 + reliable_qos: false + diff --git a/package.xml b/package.xml index 38e5150..09f9c0f 100644 --- a/package.xml +++ b/package.xml @@ -9,6 +9,10 @@ ament_cmake librapidxml-dev + eigen + rclcpp + sensor_msgs + pcl_conversions ament_lint_auto ament_lint_common diff --git a/src/imb/ImbFormat.cpp b/src/imb/ImbFormat.cpp new file mode 100644 index 0000000..cbb79a4 --- /dev/null +++ b/src/imb/ImbFormat.cpp @@ -0,0 +1,108 @@ +#include + + +namespace m3 { +namespace imb { + +PacketHeader::PacketHeader(const uint8_t* byteArray) +{ + std::memcpy(this, byteArray, sizeof(PacketHeader)); +} + +DataHeader::DataHeader(const uint8_t* byte_array){ + std::memcpy(this, byte_array, sizeof(DataHeader)); +} + + + + +DataBody::DataBody(const uint8_t* byteArray, uint16_t nNumBeams, uint16_t nNumSamples, DataType dataType) + : dataType(dataType) +{ + + switch(dataType) { + case DataType::FloatComplex: + size = nNumBeams * nNumSamples * sizeof(std::complex); + + complexData = Eigen::Matrix, Eigen::Dynamic, Eigen::Dynamic>(nNumBeams, nNumSamples); + for (size_t i = 0; i < nNumBeams; i++) { + for (size_t j = 0; j < nNumSamples; j++) { + std::complex complex; + std::memcpy(&complex, byteArray + (i * nNumSamples + j) * sizeof(std::complex), sizeof(std::complex)); + complexData(i, j) = complex; + } + } + break; + case DataType::IntegerMagnitude: + size = nNumBeams * nNumSamples * sizeof(uint8_t); + + magnitudeData = Eigen::Matrix(nNumBeams, nNumSamples); + for (size_t i = 0; i < nNumBeams; ++i) { + for (size_t j = 0; j < nNumSamples; ++j) { + magnitudeData(i, j) = byteArray[i * nNumSamples + j]; + } + } + break; + default: + throw std::runtime_error("Invalid data type"); + } +} + +PacketFooter::PacketFooter(const uint8_t* byteArray) +{ + std::memcpy(this, byteArray, sizeof(PacketFooter)); +} + +ImbPacketStructure::ImbPacketStructure(const uint8_t* byteArray) + : packet_header(byteArray) + , data_header (byteArray + sizeof(PacketHeader)) + , dataBody (byteArray + sizeof(PacketHeader) + sizeof(DataHeader), data_header.nNumBeams, data_header.nNumImageSample, packet_header.dataType) + , packet_footer(byteArray + sizeof(PacketHeader) + sizeof(DataHeader) + dataBody.size) +{ + this->Validate(); + if(!this->validated){ + return; + } + this->GeneratePointCloud(); + +} + +void ImbPacketStructure::Validate() const{ + if(this->packet_header.packet_body_size != packet_footer.packet_body_size){ + this->validated = false; + } + return; +} + +void ImbPacketStructure::GeneratePointCloud() const{ + //Inintialize the pointcloud + pcl::PointCloud cloud; + cloud.height = 1; + cloud.width = data_header.nNumBeams * data_header.nNumImageSample; + cloud.is_dense = false; + cloud.points.resize(cloud.height * cloud.width); + + for(size_t i = 0; i < data_header.nNumBeams; i++){ + float beam_angle = data_header.fBeamList[i]; //Current beam angle + for(size_t j = 0; j < data_header.nNumImageSample; j++){ + pcl::PointXYZI point; + float dist_to_point = ((data_header.fSWST - data_header.fTXWST) + data_header.fImageSampleInterval * j) + * data_header.fSoundSpeed / 2; // Formula given in the IMB format documentation + point.x = dist_to_point * cos(beam_angle * M_PI / 180.0); + point.y = dist_to_point * sin(beam_angle * M_PI / 180.0); + point.z = 0; + point.intensity = std::max(abs(dataBody.complexData(i, j)), 0.0f); + cloud.points[i * data_header.nNumImageSample + j] = point; + } + } + // Convert the pointcloud to a ROS message + sensor_msgs::msg::PointCloud2 message; + pcl::toROSMsg(cloud, message); + message.header.frame_id = "m3_sonar"; + message.header.stamp = rclcpp::Time(this->data_header.dwTimeSec, this->data_header.dwTimeMillisec); // Timestamp from sonar data + this->message = message; + return; + +} +} // namespace imb +} // namespace m3 \ No newline at end of file diff --git a/src/imb/M3Listener.cpp b/src/imb/M3Listener.cpp new file mode 100644 index 0000000..efef21d --- /dev/null +++ b/src/imb/M3Listener.cpp @@ -0,0 +1,104 @@ +#include +#include +#include + +#define SYNC_WORD_0 0x80 +#define SYNC_WORD_1 0x00 + +namespace m3{ + +M3Listener::M3Listener(std::string addr, uint16_t port, std::vector& shared_vector, std::mutex& mutex, bool& packet_ready) + : addr_ (addr), port_ (port), shared_vector_ (shared_vector), packet_ready_ (packet_ready), mutex_ (mutex) { + +} + +void M3Listener::create_socket(){ + client_socket_ = socket(AF_INET, SOCK_STREAM, 0); //Initialize socket + if (client_socket_ == -1) { + throw std::runtime_error("Error creating socket"); + } + // Set up server address information + server_addr_.sin_family = AF_INET; + server_addr_.sin_port = htons(port_); + server_addr_.sin_addr.s_addr = inet_addr(addr_.c_str()); +} + + +void M3Listener::connect_to_sonar(){ + while(true){ + std::cout << "[INFO] Attempting to connect to server " << addr_ << " at port " << port_ << std::endl; + if (connect(client_socket_, (struct sockaddr*)&server_addr_, sizeof(server_addr_)) == -1) { + int timeout_delay = 5; + std::cerr << "Error connecting to server! Trying again in " << timeout_delay << " seconds" << std::endl; + std::this_thread::sleep_until(std::chrono::system_clock::now() + std::chrono::seconds(timeout_delay)); + continue; + } + else{ + std::cout << "[INFO] Established connection with server " << addr_ << " at port " << port_ << std::endl; + break; + } + + } + +} + + +void M3Listener::run_listener() { + std::vector packet_data; //Holds all the data from each packet-collection + while (true) { + // Receive data from the server + int bytes_read = recv(client_socket_, buffer_, sizeof(buffer_), 0); //Different sizes ranging up to 2^16 + // std::cout << "Received " << bytes_read << " bytes!" << std::endl; + + if (bytes_read == -1) { //Cannot read data + std::cerr << "Error receiving data from the server" << std::endl; + connect_to_sonar(); + } else if (bytes_read == 0) { // Connection closed by the server + std::cout << "[INFO] Server closed the connection" << std::endl; + connect_to_sonar(); + } else { + buffer_[bytes_read] = '\0'; // Make sure the buffer is getting ended (should not be necessary) + + bool is_header = (int(buffer_[0]) == SYNC_WORD_1 // Check for synchronization word to find header packet + && int(buffer_[2]) == SYNC_WORD_1 + && int(buffer_[4]) == SYNC_WORD_1 + && int(buffer_[6]) == SYNC_WORD_1 + && int(buffer_[1]) == SYNC_WORD_0 + && int(buffer_[3]) == SYNC_WORD_0 + && int(buffer_[5]) == SYNC_WORD_0 + && int(buffer_[7]) == SYNC_WORD_0 + ); + if(!is_header || packet_data.size() == 0){ + for (int i = 0; i < bytes_read; i++){ // All data is stored in the vector + packet_data.push_back(buffer_[i]); + } + continue; + } + if(!packet_ready_){ + std::unique_lock lock(mutex_); // Locks the shared vector (extra protection for thread-safe handling) + shared_vector_ = packet_data; + packet_ready_ = true; + lock.unlock(); + } + packet_data.clear(); // Resets the vector for a new packet-collection + for (int i = 0; i < bytes_read; i++){ // All data is stored in the vector + packet_data.push_back(buffer_[i]); + } + } + } +} + + +void M3Listener::stop_listener(){ + std::cout << "[INFO] Closing connection to server " << addr_ << " at port " << port_ << std::endl; + close(client_socket_); +} + + +M3Listener::~M3Listener(){ + stop_listener(); +} + +} + + diff --git a/src/imb/M3Publisher.cpp b/src/imb/M3Publisher.cpp new file mode 100644 index 0000000..d7e6554 --- /dev/null +++ b/src/imb/M3Publisher.cpp @@ -0,0 +1,63 @@ +#include +using namespace std::chrono_literals; +namespace m3{ + M3Publisher::M3Publisher() : Node("M3Publisher"){ + this->declare_parameter("ip_addr", rclcpp::PARAMETER_STRING); + this->declare_parameter("port", rclcpp::PARAMETER_INTEGER); + this->declare_parameter("topic", rclcpp::PARAMETER_STRING); + this->declare_parameter("callback_time", rclcpp::PARAMETER_INTEGER); + this->declare_parameter("reliable_qos", rclcpp::PARAMETER_BOOL); + + + if(this->get_parameter("reliable_qos").as_bool()){ + // Define the quality of service profile for publisher to match the other publishers and subscribers + rmw_qos_profile_t qos_profile = rmw_qos_profile_sensor_data; + qos_profile.reliability = RMW_QOS_POLICY_RELIABILITY_BEST_EFFORT; + auto qos = rclcpp::QoS(rclcpp::QoSInitialization(qos_profile.history, 1), qos_profile); + publisher_ = this->create_publisher(this->get_parameter("topic").as_string().c_str(), qos); + } + else{ + publisher_ = this->create_publisher(this->get_parameter("topic").as_string().c_str(), 10); + } + + data_vector_ = std::vector(); + packet_ready_ = false; + std::thread(&M3Publisher::CreateListener, this, this->get_parameter("ip_addr").as_string(), this->get_parameter("port").as_int()).detach(); + timer_ = this->create_wall_timer(std::chrono::milliseconds(this->get_parameter("callback_time").as_int()), std::bind(&M3Publisher::ProcessData, this)); // How often to sample data from listener + } + void M3Publisher::ProcessData() { + try{ + if(!packet_ready_){ + return; + } + std::unique_lock lock(mutex_); // Thread-safe handling + const uint8_t* start = data_vector_.data(); // Grabs the pointer to the first element in the vector + imb::ImbPacketStructure packet(start); // Parse the packet to an object + + PublishMessage(packet.message); + + packet_ready_ = false; // Ready to receive new packet + lock.unlock(); + } + catch(const std::exception& e){ + std::cerr << e.what() << '\n'; + } + + } + void M3Publisher::PublishMessage(sensor_msgs::msg::PointCloud2 message){ + publisher_->publish(message); + } + void M3Publisher::CreateListener(std::string addr, uint16_t port){ + M3Listener listener (addr, port, data_vector_, mutex_, packet_ready_); + listener.create_socket(); + listener.connect_to_sonar(); + listener.run_listener(); + } +} + +int main(int argc, char *argv[]) { + rclcpp::init(argc, argv); + rclcpp::spin(std::make_shared()); + rclcpp::shutdown(); + return 0; +} diff --git a/src/main.cpp b/src/main.cpp deleted file mode 100644 index 2bace7c..0000000 --- a/src/main.cpp +++ /dev/null @@ -1,19 +0,0 @@ -#include -#include - -int main() -{ - std::string response = R"( - -Get_Status -OK -Pinging -2.1.0 - -)"; - - m3::api::ResponseInfo result = m3::api::parseResponse(response); - std::cout << result << std::endl; - - return 0; -} diff --git a/test/ImbFormatTest.cpp b/test/ImbFormatTest.cpp new file mode 100644 index 0000000..02641e2 --- /dev/null +++ b/test/ImbFormatTest.cpp @@ -0,0 +1,144 @@ +#include +#include +#include +#include +#include + + +TEST(ImbFormatSize, PacketHeader) { + using namespace m3::imb; + size_t size = sizeof(PacketHeader::synchronizationWord) + + sizeof(PacketHeader::dataType) + + sizeof(PacketHeader::reservedField) + + sizeof(PacketHeader::reservedBytes) + + sizeof(PacketHeader::packet_body_size); + + ASSERT_EQ(sizeof(PacketHeader), size); +} + +TEST(ImbFormatSize, DataHeader) { + size_t size = sizeof(m3::imb::DataHeader::dwBeamformed_Data_Version) + + sizeof(m3::imb::DataHeader::dwSonarID) + + sizeof(m3::imb::DataHeader::dwSonarInfo) + + sizeof(m3::imb::DataHeader::dwTimeSec) + + sizeof(m3::imb::DataHeader::dwTimeMillisec) + + sizeof(m3::imb::DataHeader::fSoundSpeed) + + sizeof(m3::imb::DataHeader::nNumImageSample) + + sizeof(m3::imb::DataHeader::fNearRangeMeter) + + sizeof(m3::imb::DataHeader::fFarRangeMeter) + + sizeof(m3::imb::DataHeader::fSWST) + + sizeof(m3::imb::DataHeader::fSWL) + + sizeof(m3::imb::DataHeader::nNumBeams) + + sizeof(m3::imb::DataHeader::wProcessingType) + + sizeof(m3::imb::DataHeader::fBeamList) + + sizeof(m3::imb::DataHeader::fImageSampleInterval) + + sizeof(m3::imb::DataHeader::nImageDestination) + + sizeof(m3::imb::DataHeader::bTXPulseType) + + sizeof(m3::imb::DataHeader::bAzimuthProcessing) + + sizeof(m3::imb::DataHeader::dwModeID) + + sizeof(m3::imb::DataHeader::nNumHybridPRIs) + + sizeof(m3::imb::DataHeader::nHybridIndex) + + sizeof(m3::imb::DataHeader::nPhaseSeqLength) + + sizeof(m3::imb::DataHeader::nPhaseSeqIndex) + + sizeof(m3::imb::DataHeader::nNumImages) + + sizeof(m3::imb::DataHeader::nSubImageIndex) + + sizeof(m3::imb::DataHeader::dwFrequency) + + sizeof(m3::imb::DataHeader::dwPulseLength) + + sizeof(m3::imb::DataHeader::dwPingNumber) + + sizeof(m3::imb::DataHeader::fRxFilterBW) + + sizeof(m3::imb::DataHeader::fRxNominalResolution) + + sizeof(m3::imb::DataHeader::fPulseRepFreq) + + sizeof(m3::imb::DataHeader::strAppName) + + sizeof(m3::imb::DataHeader::strTXPulseName) + + sizeof(m3::imb::DataHeader::sTVGParameters) + + sizeof(m3::imb::DataHeader::fCompassHeading) + + sizeof(m3::imb::DataHeader::fMagneticVariation) + + sizeof(m3::imb::DataHeader::fPitch) + + sizeof(m3::imb::DataHeader::fRoll) + + sizeof(m3::imb::DataHeader::fDepth) + + sizeof(m3::imb::DataHeader::fTemperature) + + sizeof(m3::imb::DataHeader::sOffsets) + + sizeof(m3::imb::DataHeader::dbLatitude) + + sizeof(m3::imb::DataHeader::dbLongitude) + + sizeof(m3::imb::DataHeader::fTXWST) + + sizeof(m3::imb::DataHeader::bHeadSensorVersion) + + sizeof(m3::imb::DataHeader::HeadHWStatus) + + sizeof(m3::imb::DataHeader::bSoundSpeedSource) + + sizeof(m3::imb::DataHeader::bTimeSyncMode) + + sizeof(m3::imb::DataHeader::fInternalSensorHeading) + + sizeof(m3::imb::DataHeader::fInternalSensorPitch) + + sizeof(m3::imb::DataHeader::fInternalSensorRoll) + + sizeof(m3::imb::DataHeader::sAxesRotatorOffsets) + + sizeof(m3::imb::DataHeader::nStartElement) + + sizeof(m3::imb::DataHeader::nEndElement) + + sizeof(m3::imb::DataHeader::strCustomText) + + sizeof(m3::imb::DataHeader::strROVText) + + sizeof(m3::imb::DataHeader::fLocalTimeOffset) + + sizeof(m3::imb::DataHeader::fVesselSOG) + + sizeof(m3::imb::DataHeader::fHeave) + + sizeof(m3::imb::DataHeader::fPRIMinRange) + + sizeof(m3::imb::DataHeader::fPRIMaxRange) + + sizeof(m3::imb::DataHeader::fAltitude) + + sizeof(m3::imb::DataHeader::fHeightGeoidAbvEllipsoid) + + sizeof(m3::imb::DataHeader::sRealTimeSoundSpeed) + + sizeof(m3::imb::DataHeader::sProfilingDepthTracking) + + sizeof(m3::imb::DataHeader::sGPSQualityParas) + + sizeof(m3::imb::DataHeader::b3DScan) + + sizeof(m3::imb::DataHeader::b3DScanAxis) + + sizeof(m3::imb::DataHeader::bSonarClassID) + + sizeof(m3::imb::DataHeader::byTX_Enabled) + + sizeof(m3::imb::DataHeader::bReserved) + + sizeof(m3::imb::DataHeader::fROV_Altitude) + + sizeof(m3::imb::DataHeader::fConductivity) + + sizeof(m3::imb::DataHeader::reserved); + + ASSERT_EQ(sizeof(m3::imb::DataHeader), size); +} + +TEST(ImbFormatSize, PacketFooter) { + using namespace m3::imb; + size_t size = sizeof(PacketFooter::packet_body_size) + + sizeof(PacketFooter::reserved_bytes); + + ASSERT_EQ(sizeof(PacketFooter), size); +} + +TEST(ImbFormatParsing, TestPacket){ + using namespace m3::imb; + + std::vector data; + std::ifstream file("../../install/vortex-m3-sonar-driver/data/data/test_packet.txt", std::ios::in | std::ios::binary); + if (file.is_open()) { + // Get the size of the file + file.seekg(0, std::ios::end); + std::streampos fileSize = file.tellg(); + file.seekg(0, std::ios::beg); + + // Resize the vector to hold the file contents + data.resize(static_cast(fileSize)); + + // Read the file content into the vector + file.read(reinterpret_cast(data.data()), fileSize); + + // Close the file + file.close(); + + ImbPacketStructure packet(data.data()); + + //Check synchonization words + ASSERT_EQ(packet.packet_header.synchronizationWord[0], 0x8000); + ASSERT_EQ(packet.packet_header.synchronizationWord[1], 0x8000); + ASSERT_EQ(packet.packet_header.synchronizationWord[2], 0x8000); + ASSERT_EQ(packet.packet_header.synchronizationWord[3], 0x8000); + + //Assert data size consistency + ASSERT_EQ(packet.packet_header.packet_body_size, packet.packet_footer.packet_body_size); + std::cout << std::endl; + } else { + FAIL() << "Error opening file" << std::endl; + } +} + + + + diff --git a/test/data/test_packet.txt b/test/data/test_packet.txt new file mode 100644 index 0000000..abe1ad6 Binary files /dev/null and b/test/data/test_packet.txt differ