{"id":16749,"date":"2024-10-12T16:12:52","date_gmt":"2024-10-12T08:12:52","guid":{"rendered":"http:\/\/192.168.10.115\/?p=16749"},"modified":"2024-10-12T16:12:52","modified_gmt":"2024-10-12T08:12:52","slug":"2024-10-12-autoware-universe%e9%83%a8%e7%bd%b204%ef%bc%9auniverse%e4%bc%a0%e6%84%9f%e5%99%a8ros2%e9%a9%b1%e5%8a%a8","status":"publish","type":"post","link":"http:\/\/222.128.65.89:18086\/index.php\/2024\/10\/12\/16749\/","title":{"rendered":"2024-10-12 Autoware.universe\u90e8\u7f7204\uff1auniverse\u4f20\u611f\u5668ROS2\u9a71\u52a8"},"content":{"rendered":"\n<p>\u4e00\u3001\u6fc0\u5149\u96f7\u8fbe\u9a71\u52a8<\/p>\n\n\n\n<p>\u4ee5\u901f\u817e32\u7ebf\u6fc0\u5149\u96f7\u8fbe\u4e3a\u4f8b\uff1a<br>(1) \u5efa\u7acb\u5de5\u4f5c\u7a7a\u95f4\uff0c\u4e0b\u8f7d\u4e24\u4e2a\u529f\u80fd\u5305\uff1a<\/p>\n\n\n\n<p>\u5b98\u65b9\u9a71\u52a8\u4e0b\u8f7d\u5730\u5740\uff1ahttps:\/\/github.com\/RoboSense-LiDAR\/rslidar_sdk\n\u5b98\u65b9\u96f7\u8fbeROS2\u6d88\u606f\u7c7b\u578b\u4e0b\u8f7d\u5730\u5740\uff1ahttps:\/\/github.com\/RoboSense-LiDAR\/rslidar_msg<\/p>\n\n\n\n<pre class=\"wp-block-code\"><code>mkdir -p ~\/rs_driver\/src\ncd ~\/rs_driver\/\n<\/code><\/pre>\n\n\n\n<p>2\uff09\u901f\u817e\u805a\u521b\u96f7\u8fbeROS1\uff0c<a href=\"https:\/\/so.csdn.net\/so\/search?q=ROS2&amp;spm=1001.2101.3001.7020\" target=\"_blank\" rel=\"noreferrer noopener\">ROS2<\/a>\u4ee3\u7801\u90fd\u662f\u540c\u4e00\u4e2a\u94fe\u63a5\uff0c\u6240\u4ee5\u5c06ununtu18.04\u91cc\u9762\u7528\u7684\u9a71\u52a8\u62ff\u4e86\u8fc7\u6765\uff0c\u7136\u540e\u6253\u5f00<code>senser_driver\/src\/rslidar_sdk\/CmakeLists.txt<\/code>\uff0c\u9009\u62e9CLOCON\u7f16\u8bd1\u65b9\u5f0f<\/p>\n\n\n\n<figure class=\"wp-block-image size-full\"><img loading=\"lazy\" decoding=\"async\" width=\"1002\" height=\"562\" src=\"http:\/\/192.168.10.115\/wp-content\/uploads\/2024\/10\/\u56fe\u7247-81.png\" alt=\"\" class=\"wp-image-16751\" srcset=\"http:\/\/222.128.65.89:18086\/wp-content\/uploads\/2024\/10\/\u56fe\u7247-81.png 1002w, http:\/\/222.128.65.89:18086\/wp-content\/uploads\/2024\/10\/\u56fe\u7247-81-300x168.png 300w, http:\/\/222.128.65.89:18086\/wp-content\/uploads\/2024\/10\/\u56fe\u7247-81-768x431.png 768w\" sizes=\"(max-width: 1002px) 100vw, 1002px\" \/><\/figure>\n\n\n\n<p>\uff083\uff09\u4fee\u6539\u53c2\u6570\u914d\u7f6e<br>\u4e3b\u8981\u662f\u4fee\u6539\u4e3a\u4f60\u7684lidar\u7c7b\u578b\uff0c\u5750\u6807\u7cfb\u4ee5\u53ca\u70b9\u4e91\u8bdd\u9898\uff1a<\/p>\n\n\n\n<figure class=\"wp-block-image size-large\"><img loading=\"lazy\" decoding=\"async\" width=\"1024\" height=\"446\" src=\"http:\/\/192.168.10.115\/wp-content\/uploads\/2024\/10\/\u56fe\u7247-82-1024x446.png\" alt=\"\" class=\"wp-image-16752\" srcset=\"http:\/\/222.128.65.89:18086\/wp-content\/uploads\/2024\/10\/\u56fe\u7247-82-1024x446.png 1024w, http:\/\/222.128.65.89:18086\/wp-content\/uploads\/2024\/10\/\u56fe\u7247-82-300x131.png 300w, http:\/\/222.128.65.89:18086\/wp-content\/uploads\/2024\/10\/\u56fe\u7247-82-768x335.png 768w, http:\/\/222.128.65.89:18086\/wp-content\/uploads\/2024\/10\/\u56fe\u7247-82.png 1289w\" sizes=\"(max-width: 1024px) 100vw, 1024px\" \/><\/figure>\n\n\n\n<p>\uff084\uff09\u5c06\u4e0b\u8f7d\u7684\u4e24\u4e2a\u529f\u80fd\u5305\u4e00\u8d77\u653e\u5230src\u4e0b\uff0c\u5c06\u539f\u6765\u7684package.xml\u6587\u4ef6\u91cd\u547d\u540d\u4e3apackage.xml.bak\u5907\u4efd\uff0c\u628apackage_ros2.xml\u6587\u4ef6\u91cd\u547d\u540d\u4e3apackage.xml\uff0c\u7136\u540e\u7f16\u8bd1:<\/p>\n\n\n\n<pre class=\"wp-block-code\"><code>#\u7f16\u8bd1\ncd ~\/rs_driver\/\ncolcon build\n<\/code><\/pre>\n\n\n\n<p>\uff085\uff09\u8bbe\u5907\u8fde\u63a5\u4e0e\u7f51\u7edc\u914d\u7f6e<\/p>\n\n\n\n<pre class=\"wp-block-code\"><code>\u51c6\u5907\u4e00\u5957\u901f\u817e\u805a\u521b16\u7ebf\u6fc0\u5149\u96f7\u8fbe\uff08\u5305\u62ec\u6fc0\u5149\u96f7\u8fbe\u3001\u7535\u6e90\u76d2\u5b50\u3001\u822a\u63d2\u5934\u4ee5\u53ca\u7f51\u7ebf\uff09\uff1b\n\u672c\u6587\u4f7f\u7528\u7684PC\u7cfb\u7edf\u662fUbuntu 18.04\u7cfb\u7edf\uff0c\u4e5f\u53ef\u4f7f\u7528Ubuntu 16.04\u6216Ubuntu 20.04\uff1b\n\u51c6\u5907AC 220V\u7535\u6e90\u6216DC 12V\u7535\u6e90\uff1b<\/code><\/pre>\n\n\n\n<p>\u5982\u4e0b\u56fe\u6240\u793a\uff0c\u5c06\u96f7\u8fbe\u4e00\u7aef\u7684\u822a\u63d2\u5934\u63a5\u53e3\u4e0e\u96f7\u8fbe\u7535\u6e90\u76d2\u5b50\u7684\u822a\u63d2\u5934\u63a5\u53e3\u4e24\u4e2a\uff0c\u5bf9\u51c6\u7ea2\u70b9\u63a5\u597d\uff1b<\/p>\n\n\n\n<figure class=\"wp-block-image size-full\"><img loading=\"lazy\" decoding=\"async\" width=\"360\" height=\"192\" src=\"http:\/\/192.168.10.115\/wp-content\/uploads\/2024\/10\/\u56fe\u7247-83.png\" alt=\"\" class=\"wp-image-16753\" srcset=\"http:\/\/222.128.65.89:18086\/wp-content\/uploads\/2024\/10\/\u56fe\u7247-83.png 360w, http:\/\/222.128.65.89:18086\/wp-content\/uploads\/2024\/10\/\u56fe\u7247-83-300x160.png 300w\" sizes=\"(max-width: 360px) 100vw, 360px\" \/><\/figure>\n\n\n\n<p>\u7535\u6e90\u76d2\u5b50\u63a5\u4e0a\u7535\u6e90\uff0c\u63a5\u4e0a\u7f51\u7ebf\uff08\u7f51\u7ebf\u4e00\u7aef\u63a5\u5165\u5230PC\uff0c\u4e00\u7aef\u63a5\u5165\u5230\u7535\u6e90\u76d2\u5b50\uff09\u5982\u4e0b\u56fe\uff1a<\/p>\n\n\n\n<figure class=\"wp-block-image size-full\"><img loading=\"lazy\" decoding=\"async\" width=\"360\" height=\"246\" src=\"http:\/\/192.168.10.115\/wp-content\/uploads\/2024\/10\/\u56fe\u7247-84.png\" alt=\"\" class=\"wp-image-16754\" srcset=\"http:\/\/222.128.65.89:18086\/wp-content\/uploads\/2024\/10\/\u56fe\u7247-84.png 360w, http:\/\/222.128.65.89:18086\/wp-content\/uploads\/2024\/10\/\u56fe\u7247-84-300x205.png 300w\" sizes=\"(max-width: 360px) 100vw, 360px\" \/><\/figure>\n\n\n\n<p>\u96f7\u8fbe\u70b9\u4e91\u76d2\u5b50\u8fde\u63a5\u96f7\u8fbe\u3001\u70b9\u4e91\u4ee5\u53ca\u7f51\u7ebf\uff0c\u7f51\u7ebf\u53e6\u4e00\u7aef\u8fde\u63a5\u8ba1\u7b97\u673a\uff08\u6216\u8005\u901a\u8fc7\u4ea4\u6362\u673a\u8f6c\u63a5\uff09\uff0c\u8bbe\u7f6e\u7f51\u7edc\u4e3a\u9759\u6001IP\uff0cIP\u5730\u5740\uff1a192.168.1.102\uff0c\u5b50\u7f51\u63a9\u7801\uff1a255.255.255.0<\/p>\n\n\n\n<figure class=\"wp-block-image size-full\"><img loading=\"lazy\" decoding=\"async\" width=\"662\" height=\"429\" src=\"http:\/\/192.168.10.115\/wp-content\/uploads\/2024\/10\/\u56fe\u7247-85.png\" alt=\"\" class=\"wp-image-16755\" srcset=\"http:\/\/222.128.65.89:18086\/wp-content\/uploads\/2024\/10\/\u56fe\u7247-85.png 662w, http:\/\/222.128.65.89:18086\/wp-content\/uploads\/2024\/10\/\u56fe\u7247-85-300x194.png 300w\" sizes=\"(max-width: 662px) 100vw, 662px\" \/><\/figure>\n\n\n\n<p>\uff086\uff09\u9a71\u52a8\u96f7\u8fbe<\/p>\n\n\n\n<pre class=\"wp-block-code\"><code>source install\/setup.bash<br>ros2 launch rslidar_sdk start.py<\/code><\/pre>\n\n\n\n<p>\u5728\u6253\u5f00\u7684rviz2\u4e2d\uff0cFrame\u5750\u6807\u7cfb\u6539\u6210velodyne\uff0c\u70b9\u4e91\u8bdd\u9898\u9009\u62e9\/points_raw\uff0c\u53ef\u4ee5\u6210\u529f\u663e\u793a\u96f7\u8fbe\u70b9\u4e91<\/p>\n\n\n\n<figure class=\"wp-block-image size-large\"><img loading=\"lazy\" decoding=\"async\" width=\"1024\" height=\"591\" src=\"http:\/\/192.168.10.115\/wp-content\/uploads\/2024\/10\/\u56fe\u7247-86-1024x591.png\" alt=\"\" class=\"wp-image-16756\" srcset=\"http:\/\/222.128.65.89:18086\/wp-content\/uploads\/2024\/10\/\u56fe\u7247-86-1024x591.png 1024w, http:\/\/222.128.65.89:18086\/wp-content\/uploads\/2024\/10\/\u56fe\u7247-86-300x173.png 300w, http:\/\/222.128.65.89:18086\/wp-content\/uploads\/2024\/10\/\u56fe\u7247-86-768x443.png 768w, http:\/\/222.128.65.89:18086\/wp-content\/uploads\/2024\/10\/\u56fe\u7247-86-1536x887.png 1536w, http:\/\/222.128.65.89:18086\/wp-content\/uploads\/2024\/10\/\u56fe\u7247-86-1733x1000.png 1733w, http:\/\/222.128.65.89:18086\/wp-content\/uploads\/2024\/10\/\u56fe\u7247-86.png 1873w\" sizes=\"(max-width: 1024px) 100vw, 1024px\" \/><\/figure>\n\n\n\n<p>\u4e8c\u3001IMU\u9a71\u52a8<\/p>\n\n\n\n<p>\u672c\u6587\u8bbe\u5907\u4e3a\u4e9a\u4f2f\u667a\u80fd\u5341\u8f74IMU\u60ef\u5bfc\u6a21\u5757<\/p>\n\n\n\n<p>2.1 \u4e0a\u4f4d\u673a\u914d\u7f6e<\/p>\n\n\n\n<p>\uff081\uff09 \u5b89\u88c5\u4e32\u53e3\u9a71\u52a8<br>\u6253\u5f00WIndows\uff0c\u5728\u914d\u5957\u7684\u8d44\u6599\u4e2d\u627e\u5230CP2102_USB\u9a71\u52a8.zip\u6587\u4ef6\uff0c\u4e0b\u8f7d\u5230\u672c\u5730\u7535\u8111\u5e76\u89e3\u538b\uff0c\u53cc\u51fbCP210xVCPInstaller_x64.exe\u6587\u4ef6\u6253\u5f00\u5b89\u88c5\u7a0b\u5e8f\uff0c\u7136\u540e\u8ddf\u7740\u63d0\u793a\u4e00\u76f4\u70b9\u51fb\u4e0b\u4e00\u6b65\uff0c\u76f4\u5230\u5b8c\u6210\u5373\u53ef\u3002<br>\uff082\uff09\u8fde\u63a5\u4e0a\u4f4d\u673a<br>\u89e3\u538b\u8d44\u6599\u4e2d\u7684IMU\u6807\u51c6\u4e0a\u4f4d\u673a(V6.2.60).zip\u538b\u7f29\u5305\uff0c\u8fdb\u5165\u4e0a\u4f4d\u673a\u8f6f\u4ef6\u627e\u5230MiniIMU.exe\u3002\u53cc\u51fb\u6253\u5f00\u4e0a\u4f4d\u673a\u8f6f\u4ef6\uff0c\u53ef\u4ee5\u770b\u5230\u63d0\u793a\u672a\u80fd\u641c\u7d22\u5230\u8bbe\u5907\uff0c\u5173\u95ed\u63d0\u793a\u6846\u3002\u5982\u679c\u5df2\u7ecf\u8fde\u63a5IMU\u6a21\u5757\u4f1a\u81ea\u52a8\u8fde\u63a5\u4e0a\u6a21\u5757\u3002<\/p>\n\n\n\n<p>\u5c06IMU\u6a21\u5757\u901a\u8fc7USB-TypeC\u6570\u636e\u7ebf\u8fde\u63a5\u5230\u7535\u8111\u4e0a\u3002\u7136\u540e\u70b9\u51fb\u7136\u540e\u70b9\u51fb\u83dc\u5355\u680f\u7684\u2018\u81ea\u52a8\u76d1\u6d4b\u8bbe\u5907\u2019\u3002\u8fde\u63a5\u6210\u529f\uff0c\u53ef\u4ee5\u770b\u5230\u89d2\u5ea6X\u3001\u89d2\u5ea6Y\u3001\u89d2\u5ea6Z\u6709\u6570\u636e\u53d8\u5316\u3002\u5982\u679c\u8fde\u63a5\u4e0d\u6210\u529f\uff0c\u8bf7\u786e\u8ba4\u662f\u5426\u5df2\u7ecf\u5b89\u88c5\u597d\u4e32\u53e3\u9a71\u52a8\uff0c\u6216\u8005\u6362\u4e2aUSB\u53e3\u8bd5\u8bd5\u3002<\/p>\n\n\n\n<figure class=\"wp-block-image size-large\"><img loading=\"lazy\" decoding=\"async\" width=\"1024\" height=\"686\" src=\"http:\/\/192.168.10.115\/wp-content\/uploads\/2024\/10\/\u56fe\u7247-87-1024x686.png\" alt=\"\" class=\"wp-image-16757\" srcset=\"http:\/\/222.128.65.89:18086\/wp-content\/uploads\/2024\/10\/\u56fe\u7247-87-1024x686.png 1024w, http:\/\/222.128.65.89:18086\/wp-content\/uploads\/2024\/10\/\u56fe\u7247-87-300x201.png 300w, http:\/\/222.128.65.89:18086\/wp-content\/uploads\/2024\/10\/\u56fe\u7247-87-768x515.png 768w, http:\/\/222.128.65.89:18086\/wp-content\/uploads\/2024\/10\/\u56fe\u7247-87-1536x1029.png 1536w, http:\/\/222.128.65.89:18086\/wp-content\/uploads\/2024\/10\/\u56fe\u7247-87-1492x1000.png 1492w, http:\/\/222.128.65.89:18086\/wp-content\/uploads\/2024\/10\/\u56fe\u7247-87.png 1586w\" sizes=\"(max-width: 1024px) 100vw, 1024px\" \/><\/figure>\n\n\n\n<p>IMU\u6a21\u5757\u51fa\u5382\u5df2\u7ecf\u70e7\u5f55\u597d\u56fa\u4ef6\uff0c\u8fde\u63a5\u4e0a\u4f4d\u673a\u540e\u53ef\u4ee5\u7528\u4e0a\u4f4d\u673a\u67e5\u770bIMU\u6a21\u5757\u7684\u5f53\u524d\u59ff\u6001\u3002<br>\u70b9\u51fb\u2019\u4e09\u7ef4\u2018\uff0c\u53ef\u4ee5\u770b\u5230\u5f39\u51fa\u4e00\u4e2a\u7a97\u53e3\uff0c\u9ed8\u8ba4\u4f1a\u5c55\u793a\u4e00\u53f0\u6c7d\u8f66\u6a21\u578b\uff0c\u5f53\u6211\u4eec\u6539\u53d8IMU\u6a21\u5757\u7684\u59ff\u6001\u65f6\uff0c\u6a21\u578b\u7684\u59ff\u6001\u4f1a\u8ddf\u7740IMU\u6a21\u5757\u7684\u53d8\u5316\u3002Y\u8f74\u4e3a\u8f66\u5934\uff0cIMU\u6a21\u5757\u4e5f\u5e94Y\u8f74\u5411\u524d\u653e\u7f6e\u3002<\/p>\n\n\n\n<figure class=\"wp-block-image size-large\"><img loading=\"lazy\" decoding=\"async\" width=\"1024\" height=\"595\" src=\"http:\/\/192.168.10.115\/wp-content\/uploads\/2024\/10\/\u56fe\u7247-88-1024x595.png\" alt=\"\" class=\"wp-image-16758\" srcset=\"http:\/\/222.128.65.89:18086\/wp-content\/uploads\/2024\/10\/\u56fe\u7247-88-1024x595.png 1024w, http:\/\/222.128.65.89:18086\/wp-content\/uploads\/2024\/10\/\u56fe\u7247-88-300x174.png 300w, http:\/\/222.128.65.89:18086\/wp-content\/uploads\/2024\/10\/\u56fe\u7247-88-768x446.png 768w, http:\/\/222.128.65.89:18086\/wp-content\/uploads\/2024\/10\/\u56fe\u7247-88.png 1028w\" sizes=\"(max-width: 1024px) 100vw, 1024px\" \/><\/figure>\n\n\n\n<p>\uff083\uff09\u53c2\u6570\u914d\u7f6e<br>\u70b9\u51fb\u83dc\u5355\u680f\u4e0a\u7684\u2018\u914d\u7f6e\u2019\uff0c\u4f1a\u5f39\u51fa\u4e00\u4e2a\u7a97\u53e3\uff0c\u67e5\u770b\u53f3\u4e0b\u89d2\u7684\u72b6\u6001\uff0c\u4e00\u5b9a\u8981\u662f\u2018\u5728\u7ebf\u2019\u624d\u662f\u6b63\u786e\u7684\uff0c\u5982\u679c\u51fa\u73b0\u2018\u79bb \u7ebf\u2019\u5219\u8bf4\u660e\u6ca1\u8fde\u63a5\u4e0aIMU\u6a21\u5757\u3002<br>\u901a\u8baf\u901f\u7387\uff1a\u4e32\u53e3\u901a\u8baf\u901f\u7387\uff0c\u9ed8\u8ba49600\uff0c\u53ef\u9009\u62e9\u5176\u4ed6\u6ce2\u7279\u7387\uff084800~921600\uff09\u3002<br>\u56de\u4f20\u901f\u7387\uff1a\u4e32\u53e3\u56de\u4f20\u6570\u636e\u7684\u901f\u7387\uff0c\u9ed8\u8ba4\u4e3a10Hz\uff0c\u53ef\u4fee\u6539\u4e3a0.2Hz~200Hz\u300210HZ\u6307\u7684\u662f1S\u56de\u4f2010\u4e2a\u6570\u636e\u5305\uff0c\u6309\u9ed8\u8ba4\u56de\u4f201\u4e2a\u6570\u636e\u5305\u662f11\u4e2a\u5b57\u8282\u3002<br>\u6ce8\uff1a\u5982\u679c\u9700\u8981200HZ\u7684\u56de\u4f20\u901f\u7387\uff0c\u5219\u53ea\u80fd\u52fe\u9009\u4e09\u4e2a\u91cf\uff0c\u6bd4\u5982\u201c\u52a0\u901f\u5ea6\u201d\uff0c\u201c\u89d2\u901f\u5ea6\u201d\uff0c\u201c\u89d2\u5ea6\u201d\u3002<br>\u6ce8\uff1a\u5982\u679c\u56de\u4f20\u5185\u5bb9\u8f83\u591a\uff0c\u540c\u65f6\u901a\u4fe1\u7684\u6ce2\u7279\u7387\u53c8\u8f83\u4f4e\u7684\u60c5\u51b5\u4e0b\uff0c\u53ef\u80fd\u6ca1\u6cd5\u4f20\u8f93\u8fd9\u4e48\u591a\u6570\u636e\uff0c\u6b64\u65f6\u6a21\u5757\u4f1a\u81ea\u52a8\u964d\u9891\uff0c\u5e76\u4ee5\u5141\u8bb8\u7684\u6700\u5927\u8f93\u51fa\u901f\u7387\u8fdb\u884c\u8f93\u51fa\u3002\u7b80\u5355\u70b9\u8bf4 \u5c31\u662f\u56de\u4f20\u901f\u7387\u9ad8\u7684\u8bdd\uff0c\u6ce2\u7279\u7387\u4e5f\u8981\u8bbe\u7f6e\u9ad8\u4e00\u70b9\uff0c\u4e00\u822c\u7528115200\u3002\u8fd9\u91cc\u6211\u4eec\u6ce2\u7279\u7387\u6539\u4e3a480600\uff0c\u56de\u4f20\u901f\u7387\u6539\u4e3a100Hz<\/p>\n\n\n\n<figure class=\"wp-block-image size-large\"><img loading=\"lazy\" decoding=\"async\" width=\"1024\" height=\"917\" src=\"http:\/\/192.168.10.115\/wp-content\/uploads\/2024\/10\/\u56fe\u7247-89-1024x917.png\" alt=\"\" class=\"wp-image-16759\" srcset=\"http:\/\/222.128.65.89:18086\/wp-content\/uploads\/2024\/10\/\u56fe\u7247-89-1024x917.png 1024w, http:\/\/222.128.65.89:18086\/wp-content\/uploads\/2024\/10\/\u56fe\u7247-89-300x269.png 300w, http:\/\/222.128.65.89:18086\/wp-content\/uploads\/2024\/10\/\u56fe\u7247-89-768x688.png 768w, http:\/\/222.128.65.89:18086\/wp-content\/uploads\/2024\/10\/\u56fe\u7247-89.png 1111w\" sizes=\"(max-width: 1024px) 100vw, 1024px\" \/><\/figure>\n\n\n\n<p>\u8bbe\u7f6e\u4e32\u53e3\u8f93\u51fa\u7684\u5185\u5bb9\uff0c\u4e32\u53e3\u8f93\u51fa\u5185\u5bb9\u53ef\u67e5\u770b\u534f\u8bae\u6587\u4ef6\u89e3\u6790\u6570\u636e\u3002<br>\u6ce8\u610f\uff1a\u52fe\u9009\u4e0a\u201cGPS\u539f\u59cb\u201d\u4e4b\u540e\u6a21\u5757\u53ea\u8f93\u51faGPS\u539f\u59cb\u7684\u4fe1\u606f\u4e86\uff0c\u5176\u5b83\u6570\u636e\u90fd\u4e0d\u4f1a\u8f93\u51fa\u3002<\/p>\n\n\n\n<p>4.2 IMU\u6821\u51c6<\/p>\n\n\n\n<p>\uff081\uff09\u52a0\u901f\u5ea6\u8ba1\u6821\u51c6<br>\u5c06IMU\u6a21\u5757\u5e73\u653e\u5728\u684c\u5b50\u6216\u8005\u5176\u4ed6\u8bbe\u5907\u4e0a\uff0c\u5982\u679c\u53d1\u73b0\u2018\u89d2\u5ea6X\u2018\u548c\u2019\u89d2\u5ea6Y\u2018\u5927\u4e8e1\u00b0\uff0c\u90a3\u4e48\u9700\u8981\u8fdb\u884c\u52a0\u901f\u5ea6\u8ba1\u6821\u51c6\u3002\u70b9\u51fb\u83dc\u5355\u680f\u4e2d\u7684\u2019\u914d\u7f6e\u2018\u6253\u5f00\u914d\u7f6e\u754c\u9762\uff0c\u4fdd\u8bc1IMU\u6a21\u5757\u5e73\u653e\u7684\u60c5\u51b5\u4e0b\uff0c\u70b9\u51fb\u2019\u52a0\u901f\u5ea6\u2018\uff0c\u7136\u540e\u518d\u70b9\u51fb\u2019\u8bbe\u7f6e\u89d2\u5ea6\u53c2\u8003\u2018\u3002<\/p>\n\n\n\n<figure class=\"wp-block-image size-large\"><img loading=\"lazy\" decoding=\"async\" width=\"1024\" height=\"917\" src=\"http:\/\/192.168.10.115\/wp-content\/uploads\/2024\/10\/\u56fe\u7247-90-1024x917.png\" alt=\"\" class=\"wp-image-16760\" srcset=\"http:\/\/222.128.65.89:18086\/wp-content\/uploads\/2024\/10\/\u56fe\u7247-90-1024x917.png 1024w, http:\/\/222.128.65.89:18086\/wp-content\/uploads\/2024\/10\/\u56fe\u7247-90-300x269.png 300w, http:\/\/222.128.65.89:18086\/wp-content\/uploads\/2024\/10\/\u56fe\u7247-90-768x688.png 768w, http:\/\/222.128.65.89:18086\/wp-content\/uploads\/2024\/10\/\u56fe\u7247-90.png 1111w\" sizes=\"(max-width: 1024px) 100vw, 1024px\" \/><\/figure>\n\n\n\n<p>\u6821\u51c6\u4e4b\u540e\u53ef\u4ee5\u770b\u5230\u2018\u89d2\u5ea6X\u2018\u548c\u2019\u89d2\u5ea6Y\u2018\u63a5\u8fd1\u4e8e0\u00b0<\/p>\n\n\n\n<figure class=\"wp-block-image size-large\"><img loading=\"lazy\" decoding=\"async\" width=\"1024\" height=\"688\" src=\"http:\/\/192.168.10.115\/wp-content\/uploads\/2024\/10\/\u56fe\u7247-91-1024x688.png\" alt=\"\" class=\"wp-image-16761\" srcset=\"http:\/\/222.128.65.89:18086\/wp-content\/uploads\/2024\/10\/\u56fe\u7247-91-1024x688.png 1024w, http:\/\/222.128.65.89:18086\/wp-content\/uploads\/2024\/10\/\u56fe\u7247-91-300x201.png 300w, http:\/\/222.128.65.89:18086\/wp-content\/uploads\/2024\/10\/\u56fe\u7247-91-768x516.png 768w, http:\/\/222.128.65.89:18086\/wp-content\/uploads\/2024\/10\/\u56fe\u7247-91-1536x1031.png 1536w, http:\/\/222.128.65.89:18086\/wp-content\/uploads\/2024\/10\/\u56fe\u7247-91-1489x1000.png 1489w, http:\/\/222.128.65.89:18086\/wp-content\/uploads\/2024\/10\/\u56fe\u7247-91.png 1586w\" sizes=\"(max-width: 1024px) 100vw, 1024px\" \/><\/figure>\n\n\n\n<p>\uff082\uff09IMU\u6a21\u5757\u4e0a\u7535\u540e\uff0c\u6253\u5f00\u4e0a\u4f4d\u673a\u663e\u793a3D\u6a21\u578b\uff0c\u8f6c\u52a8\u6a21\u5757Z\u8f74\u822a\u5411\u89d2\uff0c3D\u6a21\u578b\u6296\u52a8\uff0c\u6216\u8005\u53cd\u5e94\u8fdf\u949d\uff0c\u8bf7\u5728\u4e0a<br>\u4f4d\u673a\u8fdb\u884c\u78c1\u529b\u6821\u51c6\u8bbe\u5907\u3002<br>\u70b9\u51fb\u914d\u7f6e\u754c\u9762\u4e2d\u7684\u2018\u78c1\u573a\u2019\uff0c\u4f1a\u5f39\u51fa\u6821\u51c6\u78c1\u573a\u7684\u754c\u9762\u3002\u78c1\u573a\u6821\u51c6\u6709\u591a\u79cd\u6821\u51c6\u65b9\u5f0f\uff0c\u6bd4\u8f83\u5e38\u89c4\u7684\u6821\u51c6\u65b9\u5f0f\u4e3a\u7403\u5f62\u62df\u5408\u6cd5\u3002<\/p>\n\n\n\n<figure class=\"wp-block-image size-full\"><img loading=\"lazy\" decoding=\"async\" width=\"794\" height=\"632\" src=\"http:\/\/192.168.10.115\/wp-content\/uploads\/2024\/10\/\u56fe\u7247-92.png\" alt=\"\" class=\"wp-image-16762\" srcset=\"http:\/\/222.128.65.89:18086\/wp-content\/uploads\/2024\/10\/\u56fe\u7247-92.png 794w, http:\/\/222.128.65.89:18086\/wp-content\/uploads\/2024\/10\/\u56fe\u7247-92-300x239.png 300w, http:\/\/222.128.65.89:18086\/wp-content\/uploads\/2024\/10\/\u56fe\u7247-92-768x611.png 768w\" sizes=\"(max-width: 794px) 100vw, 794px\" \/><\/figure>\n\n\n\n<p>\u5206\u522b\u6821\u51c6X\/Y\/Z\u8f74\uff0c\u770bchartYZ\/chartXZ\/chartXY\u53d8\u5316\u3002\u5c06IMU\u6a21\u5757\u6c34\u5e73\u653e\u7f6e\u3002\u7136\u540e\u7f13\u6162\u7ed5X\/Y\/Z\u8f74\u65cb\u8f6c360\u00b0\u4ee5<br>\u4e0a\uff0cchart\u754c\u9762\u84dd\u8272\u6570\u636e\u5206\u5e03\u5728\u7eff\u7ebf\u65c1\u4e3a\u6b63\u5e38\u3002\u4e3a\u4e86\u6570\u636e\u66f4\u52a0\u51c6\u786e\uff0c\u53ef\u591a\u8f6c\u51e0\u5708\u3002<\/p>\n\n\n\n<figure class=\"wp-block-image size-large\"><img loading=\"lazy\" decoding=\"async\" width=\"1024\" height=\"875\" src=\"http:\/\/192.168.10.115\/wp-content\/uploads\/2024\/10\/\u56fe\u7247-93-1024x875.png\" alt=\"\" class=\"wp-image-16763\" srcset=\"http:\/\/222.128.65.89:18086\/wp-content\/uploads\/2024\/10\/\u56fe\u7247-93-1024x875.png 1024w, http:\/\/222.128.65.89:18086\/wp-content\/uploads\/2024\/10\/\u56fe\u7247-93-300x256.png 300w, http:\/\/222.128.65.89:18086\/wp-content\/uploads\/2024\/10\/\u56fe\u7247-93-768x656.png 768w, http:\/\/222.128.65.89:18086\/wp-content\/uploads\/2024\/10\/\u56fe\u7247-93-1170x1000.png 1170w, http:\/\/222.128.65.89:18086\/wp-content\/uploads\/2024\/10\/\u56fe\u7247-93.png 1370w\" sizes=\"(max-width: 1024px) 100vw, 1024px\" \/><\/figure>\n\n\n\n<p>XYZ\u4e09\u8f74\u90fd\u6821\u51c6\u5b8c\u6210\u540e\u70b9\u51fb\u2018\u7ed3\u675f\u6821\u51c6\u2019\u3002\u6ce8\u610f\uff0c\u5728\u6821\u51c6Y\u8f74\u65f6\uff0c\u53ea\u770bchartXZ\u7684\u6570\u636e\u5c31\u597d\uff0c\u5176\u4ed6\u4e24\u4e2a\u89c6\u56fe\u4e5f\u6709\u6570\u636e\uff0c\u4e0d\u9700\u8981\u5173\u5fc3\u3002\u540c\u7406\u5176\u4ed6\u4e24\u4e2a\u8f74\u4e5f\u662f\u4e00\u6837\u3002<br>\uff083\uff09\u9640\u87ba\u4eea\u6821\u51c6<br>\u9640\u87ba\u4eea\u9ed8\u8ba4\u5f00\u542f\u81ea\u52a8\u6821\u51c6\u529f\u80fd\uff0c\u4e0d\u9700\u8981\u989d\u5916\u8bbe\u7f6e\u3002\u4fdd\u6301\u5f00\u542f\u9640\u87ba\u4eea\u81ea\u52a8\u6821\u51c6\u529f\u80fd\u5373\u53ef\u3002<\/p>\n\n\n\n<figure class=\"wp-block-image size-large\"><img loading=\"lazy\" decoding=\"async\" width=\"1024\" height=\"734\" src=\"http:\/\/192.168.10.115\/wp-content\/uploads\/2024\/10\/\u56fe\u7247-94-1024x734.png\" alt=\"\" class=\"wp-image-16764\" srcset=\"http:\/\/222.128.65.89:18086\/wp-content\/uploads\/2024\/10\/\u56fe\u7247-94-1024x734.png 1024w, http:\/\/222.128.65.89:18086\/wp-content\/uploads\/2024\/10\/\u56fe\u7247-94-300x215.png 300w, http:\/\/222.128.65.89:18086\/wp-content\/uploads\/2024\/10\/\u56fe\u7247-94-768x550.png 768w, http:\/\/222.128.65.89:18086\/wp-content\/uploads\/2024\/10\/\u56fe\u7247-94.png 1097w\" sizes=\"(max-width: 1024px) 100vw, 1024px\" \/><\/figure>\n\n\n\n<p>4.3 \u5b89\u88c5ROS\u9a71\u52a8<\/p>\n\n\n\n<p>\uff081\uff09\u5b89\u88c5\u4f9d\u8d56\uff1a<\/p>\n\n\n\n<pre class=\"wp-block-code\"><code>pip3 install pyserial<\/code><\/pre>\n\n\n\n<p>\uff082\uff09\u5728\u914d\u5957\u8d44\u6599\u4e2d\u627e\u5230ROS2\u9a71\u52a8\u5305wit_ros2_imu\uff0c\u653e\u5165\u5de5\u4f5c\u7a7a\u95f4\u7f16\u8bd1\uff0c\u9047\u5230\u4ee5\u4e0b\u8b66\u544a\uff1a<\/p>\n\n\n\n<figure class=\"wp-block-image size-large\"><img loading=\"lazy\" decoding=\"async\" width=\"1024\" height=\"545\" src=\"http:\/\/192.168.10.115\/wp-content\/uploads\/2024\/10\/\u56fe\u7247-95-1024x545.png\" alt=\"\" class=\"wp-image-16765\" srcset=\"http:\/\/222.128.65.89:18086\/wp-content\/uploads\/2024\/10\/\u56fe\u7247-95-1024x545.png 1024w, http:\/\/222.128.65.89:18086\/wp-content\/uploads\/2024\/10\/\u56fe\u7247-95-300x160.png 300w, http:\/\/222.128.65.89:18086\/wp-content\/uploads\/2024\/10\/\u56fe\u7247-95-768x409.png 768w, http:\/\/222.128.65.89:18086\/wp-content\/uploads\/2024\/10\/\u56fe\u7247-95.png 1102w\" sizes=\"(max-width: 1024px) 100vw, 1024px\" \/><\/figure>\n\n\n\n<p>\u6839\u636e\u63d0\u793a\u5c06setup.cfg\u7684\u6a2a\u7ebf\u6539\u4e3a\u4e0b\u5212\u7ebf\uff1a<\/p>\n\n\n\n<figure class=\"wp-block-image size-full\"><img loading=\"lazy\" decoding=\"async\" width=\"784\" height=\"114\" src=\"http:\/\/192.168.10.115\/wp-content\/uploads\/2024\/10\/\u56fe\u7247-96.png\" alt=\"\" class=\"wp-image-16766\" srcset=\"http:\/\/222.128.65.89:18086\/wp-content\/uploads\/2024\/10\/\u56fe\u7247-96.png 784w, http:\/\/222.128.65.89:18086\/wp-content\/uploads\/2024\/10\/\u56fe\u7247-96-300x44.png 300w, http:\/\/222.128.65.89:18086\/wp-content\/uploads\/2024\/10\/\u56fe\u7247-96-768x112.png 768w\" sizes=\"(max-width: 784px) 100vw, 784px\" \/><\/figure>\n\n\n\n<p>\uff083\uff09\u7ed1\u5b9a\u7aef\u53e3<br>\u4e3a\u4e86\u9632\u6b62\u591a\u4e2ausb\u8bbe\u5907\u540c\u65f6\u63d2\u5165\u7684\u65f6\u5019\uff0c\u7cfb\u7edf\u8bc6\u522b\u9519\u8bef\uff0c\u6211\u4eec\u7ed9\u8be5\u6a21\u5757\u7684\u4e32\u53e3\u540d\u5b57\u7ed1\u5b9a\u6210\/dev\/imu_usb\uff0c\u7ec8\u7aef\u8f93\u5165<\/p>\n\n\n\n<pre class=\"wp-block-code\"><code>cd src\/wit_ros_imu\nsudo chmod 777 bind_usb.sh\nsudo sh bind_usb.sh\n<\/code><\/pre>\n\n\n\n<p>\u91cd\u65b0\u63d2\u62d4\u8fde\u63a5IMU\u6a21\u5757\u7684USB\u6570\u636e\u7ebf\u3002\u4ee5\u751f\u6548\u7ed1\u5b9a\u7684\u7aef\u53e3\uff0c\u8f93\u5165\u4ee5\u4e0b\u6307\u4ee4\u68c0\u6d4b\u7ed1\u5b9a\u7aef\u53e3\u662f\u5426\u6210\u529f\uff0c<\/p>\n\n\n\n<pre class=\"wp-block-code\"><code>ll \/dev\/imu_usb\n<\/code><\/pre>\n\n\n\n<figure class=\"wp-block-image size-large\"><img loading=\"lazy\" decoding=\"async\" width=\"1024\" height=\"325\" src=\"http:\/\/192.168.10.115\/wp-content\/uploads\/2024\/10\/\u56fe\u7247-97-1024x325.png\" alt=\"\" class=\"wp-image-16767\" srcset=\"http:\/\/222.128.65.89:18086\/wp-content\/uploads\/2024\/10\/\u56fe\u7247-97-1024x325.png 1024w, http:\/\/222.128.65.89:18086\/wp-content\/uploads\/2024\/10\/\u56fe\u7247-97-300x95.png 300w, http:\/\/222.128.65.89:18086\/wp-content\/uploads\/2024\/10\/\u56fe\u7247-97-768x244.png 768w, http:\/\/222.128.65.89:18086\/wp-content\/uploads\/2024\/10\/\u56fe\u7247-97.png 1132w\" sizes=\"(max-width: 1024px) 100vw, 1024px\" \/><\/figure>\n\n\n\n<p>\u4e0d\u4e00\u5b9a\u662fttyUSB0\uff0c\u53ea\u8981\u663e\u793a\u662fUSB\u8bbe\u5907\u5c31\u884c\u4e86\u3002\u6211\u4eec\u4e0b\u9762\u5b89\u88c5\u7684GNSS\u6a21\u5757\u4e5f\u6709\u4e00\u4e2a\u4e32\u53e3\uff0c\u8fd9\u91cc\u53ef\u4ee5\u5c06\u5176\u4e5f\u7ed9\u7ed1\u5b9a\u3002\u9996\u5148\u901a\u8fc7\u63d2\u62d4\u4e24\u4e2a\u7aef\u53e3\uff0c\u6211\u4eec\u53ef\u4ee5lsusb\u67e5\u770b\u7aef\u53e3\u4fe1\u606f\uff1a<\/p>\n\n\n\n<figure class=\"wp-block-image size-large\"><img loading=\"lazy\" decoding=\"async\" width=\"1024\" height=\"545\" src=\"http:\/\/192.168.10.115\/wp-content\/uploads\/2024\/10\/\u56fe\u7247-98-1024x545.png\" alt=\"\" class=\"wp-image-16768\" srcset=\"http:\/\/222.128.65.89:18086\/wp-content\/uploads\/2024\/10\/\u56fe\u7247-98-1024x545.png 1024w, http:\/\/222.128.65.89:18086\/wp-content\/uploads\/2024\/10\/\u56fe\u7247-98-300x160.png 300w, http:\/\/222.128.65.89:18086\/wp-content\/uploads\/2024\/10\/\u56fe\u7247-98-768x409.png 768w, http:\/\/222.128.65.89:18086\/wp-content\/uploads\/2024\/10\/\u56fe\u7247-98.png 1102w\" sizes=\"(max-width: 1024px) 100vw, 1024px\" \/><\/figure>\n\n\n\n<p>\u5176\u4e2dGNSS\u7684\u4e3a\uff1a<\/p>\n\n\n\n<pre class=\"wp-block-code\"><code>0403:6001 Future Technology Devices International, Ltd FT232 Serial (UART) IC\n<\/code><\/pre>\n\n\n\n<p>IMU\u6a21\u5757\u7684\u4e3a\uff1a<\/p>\n\n\n\n<pre class=\"wp-block-code\"><code>10c4:ea60 Silicon Labs CP210x UART Bridge\n<\/code><\/pre>\n\n\n\n<p>\u7136\u540e\u521b\u5efa.rules\u6587\u4ef6\uff08\u6216\u8005\u76f4\u63a5\u5728\u4e0a\u9762IMU\u7684\u6587\u4ef6\u4e2d\u4fee\u6539\uff09\uff0c\u586b\u5199\u4ee5\u4e0b\u5185\u5bb9<\/p>\n\n\n\n<pre class=\"wp-block-code\"><code>KERNEL==\"ttyUSB*\", ATTRS{idVendor}==\"10c4\", ATTRS{idProduct}==\"ea60\", MODE:=\"0777\", SYMLINK+=\"imu_usb\"\nKERNEL==\"ttyUSB*\", ATTRS{idVendor}==\"0403\", ATTRS{idProduct}==\"6001\", MODE:=\"0777\", SYMLINK+=\"gnss\"\n<\/code><\/pre>\n\n\n\n<figure class=\"wp-block-image size-large\"><img loading=\"lazy\" decoding=\"async\" width=\"1024\" height=\"123\" src=\"http:\/\/192.168.10.115\/wp-content\/uploads\/2024\/10\/\u56fe\u7247-99-1024x123.png\" alt=\"\" class=\"wp-image-16769\" srcset=\"http:\/\/222.128.65.89:18086\/wp-content\/uploads\/2024\/10\/\u56fe\u7247-99-1024x123.png 1024w, http:\/\/222.128.65.89:18086\/wp-content\/uploads\/2024\/10\/\u56fe\u7247-99-300x36.png 300w, http:\/\/222.128.65.89:18086\/wp-content\/uploads\/2024\/10\/\u56fe\u7247-99-768x92.png 768w, http:\/\/222.128.65.89:18086\/wp-content\/uploads\/2024\/10\/\u56fe\u7247-99.png 1083w\" sizes=\"(max-width: 1024px) 100vw, 1024px\" \/><\/figure>\n\n\n\n<p>\u7136\u540e\uff1a<\/p>\n\n\n\n<pre class=\"wp-block-code\"><code>sudo cp imu_usb.rules \/etc\/udev\/rules.d\nservice udev reload\nservice udev restart\n<\/code><\/pre>\n\n\n\n<p>\u8f93\u5165\u4ee5\u4e0b\u6307\u4ee4\u68c0\u6d4b\u7ed1\u5b9a\u7aef\u53e3\u662f\u5426\u6210\u529f\uff0c<\/p>\n\n\n\n<pre class=\"wp-block-code\"><code>ll \/dev\/imu_usb\nll \/dev\/gnss\n<\/code><\/pre>\n\n\n\n<figure class=\"wp-block-image size-large\"><img loading=\"lazy\" decoding=\"async\" width=\"1024\" height=\"545\" src=\"http:\/\/192.168.10.115\/wp-content\/uploads\/2024\/10\/\u56fe\u7247-100-1024x545.png\" alt=\"\" class=\"wp-image-16770\" srcset=\"http:\/\/222.128.65.89:18086\/wp-content\/uploads\/2024\/10\/\u56fe\u7247-100-1024x545.png 1024w, http:\/\/222.128.65.89:18086\/wp-content\/uploads\/2024\/10\/\u56fe\u7247-100-300x160.png 300w, http:\/\/222.128.65.89:18086\/wp-content\/uploads\/2024\/10\/\u56fe\u7247-100-768x409.png 768w, http:\/\/222.128.65.89:18086\/wp-content\/uploads\/2024\/10\/\u56fe\u7247-100.png 1102w\" sizes=\"(max-width: 1024px) 100vw, 1024px\" \/><\/figure>\n\n\n\n<p>\u8bb0\u5f97\u628aGNSS\u7aef\u53e3\u6539\u6210\u6211\u4eec\u7ed1\u5b9a\u7684<br>\uff084\uff09\u4fee\u6539\u6ce2\u7279\u7387<br>\u7a0b\u5e8f\u9ed8\u8ba4\u662f\u4f7f\u75289600\u7684\u6ce2\u7279\u7387\uff0c\u6211\u4eec\u5728\u4e0a\u4f4d\u673a\u4fee\u6539\u4e86\u6ce2\u7279\u7387460800\uff0c\u90a3\u4e48\u5219\u9700\u8981\u4fee\u6539\u6e90\u7801\u4e2d\u7684\u6ce2\u7279\u7387\uff0c\u6e90\u7801\u4fee\u6539\u6ce2\u7279\u7387\u7684\u4f4d\u7f6e\u662f\uff0cwit_ros2_imu\/wit_ros2_imu\/wit_ros2_imu.py<\/p>\n\n\n\n<pre class=\"wp-block-code\"><code>#149\u884c\ndef driver_loop(self, port_name):\n# \u6253\u5f00\u4e32\u53e3\n    try:\n        wt_imu = serial.Serial(port=\"\/dev\/imu_usb\", baudrate=460800, timeout=0.5)\n<\/code><\/pre>\n\n\n\n<p>\u628a9600\u6539\u6210\u4e0a\u4f4d\u673a\u4e0a\u4fee\u6539\u7684\u6ce2\u7279\u7387\uff0c\u7136\u540e\u4fdd\u5b58\u540e\u9000\u51fa\uff0c\u6700\u540e\u56de\u5230\u5de5\u4f5c\u7a7a\u95f4\u76ee\u5f55\u4e0b\u8fdb\u884c\u7f16\u8bd1\u5373\u53ef\u3002<br>\uff085\uff09\u8fd0\u884c\u6d4b\u8bd5<\/p>\n\n\n\n<pre class=\"wp-block-code\"><code>source install\/setup.bash<br>ros2 launch wit_ros2_imu rviz_and_imu.launch.py<\/code><\/pre>\n\n\n\n<p>\u62a5\u4e0b\u9762\u7684\u9519\u8bef\uff0c\u662f\u56e0\u4e3alaunch\u8bed\u6cd5\u7684\u95ee\u9898\uff0c\u53ef\u80fd\u662f\u5b98\u65b9\u4f7f\u7528\u7684ROS\u7248\u672c\u6bd4\u8f83\u8001<\/p>\n\n\n\n<figure class=\"wp-block-image size-large\"><img loading=\"lazy\" decoding=\"async\" width=\"1024\" height=\"618\" src=\"http:\/\/192.168.10.115\/wp-content\/uploads\/2024\/10\/\u56fe\u7247-101-1024x618.png\" alt=\"\" class=\"wp-image-16771\" srcset=\"http:\/\/222.128.65.89:18086\/wp-content\/uploads\/2024\/10\/\u56fe\u7247-101-1024x618.png 1024w, http:\/\/222.128.65.89:18086\/wp-content\/uploads\/2024\/10\/\u56fe\u7247-101-300x181.png 300w, http:\/\/222.128.65.89:18086\/wp-content\/uploads\/2024\/10\/\u56fe\u7247-101-768x464.png 768w, http:\/\/222.128.65.89:18086\/wp-content\/uploads\/2024\/10\/\u56fe\u7247-101-1536x927.png 1536w, http:\/\/222.128.65.89:18086\/wp-content\/uploads\/2024\/10\/\u56fe\u7247-101-1657x1000.png 1657w, http:\/\/222.128.65.89:18086\/wp-content\/uploads\/2024\/10\/\u56fe\u7247-101.png 1794w\" sizes=\"(max-width: 1024px) 100vw, 1024px\" \/><\/figure>\n\n\n\n<p>\u4fee\u6539launch\uff0c\u5e76\u91cd\u65b0\u7f16\u8bd1\uff1a<\/p>\n\n\n\n<figure class=\"wp-block-image size-full\"><img loading=\"lazy\" decoding=\"async\" width=\"839\" height=\"789\" src=\"http:\/\/192.168.10.115\/wp-content\/uploads\/2024\/10\/\u56fe\u7247-102.png\" alt=\"\" class=\"wp-image-16772\" srcset=\"http:\/\/222.128.65.89:18086\/wp-content\/uploads\/2024\/10\/\u56fe\u7247-102.png 839w, http:\/\/222.128.65.89:18086\/wp-content\/uploads\/2024\/10\/\u56fe\u7247-102-300x282.png 300w, http:\/\/222.128.65.89:18086\/wp-content\/uploads\/2024\/10\/\u56fe\u7247-102-768x722.png 768w\" sizes=\"(max-width: 839px) 100vw, 839px\" \/><\/figure>\n\n\n\n<p>\u518d\u6b21\u8fd0\u884c<\/p>\n\n\n\n<figure class=\"wp-block-image size-large\"><img loading=\"lazy\" decoding=\"async\" width=\"1024\" height=\"350\" src=\"http:\/\/192.168.10.115\/wp-content\/uploads\/2024\/10\/\u56fe\u7247-103-1024x350.png\" alt=\"\" class=\"wp-image-16773\" srcset=\"http:\/\/222.128.65.89:18086\/wp-content\/uploads\/2024\/10\/\u56fe\u7247-103-1024x350.png 1024w, http:\/\/222.128.65.89:18086\/wp-content\/uploads\/2024\/10\/\u56fe\u7247-103-300x102.png 300w, http:\/\/222.128.65.89:18086\/wp-content\/uploads\/2024\/10\/\u56fe\u7247-103-768x262.png 768w, http:\/\/222.128.65.89:18086\/wp-content\/uploads\/2024\/10\/\u56fe\u7247-103.png 1162w\" sizes=\"(max-width: 1024px) 100vw, 1024px\" \/><\/figure>\n\n\n\n<p>\u67e5\u770bIMU\u8bdd\u9898\uff1a<\/p>\n\n\n\n<p>ros2 topic echo \/imu\/data_raw<\/p>\n\n\n\n<figure class=\"wp-block-image size-full\"><img loading=\"lazy\" decoding=\"async\" width=\"702\" height=\"1024\" src=\"http:\/\/192.168.10.115\/wp-content\/uploads\/2024\/10\/\u56fe\u7247-104.png\" alt=\"\" class=\"wp-image-16774\" srcset=\"http:\/\/222.128.65.89:18086\/wp-content\/uploads\/2024\/10\/\u56fe\u7247-104.png 702w, http:\/\/222.128.65.89:18086\/wp-content\/uploads\/2024\/10\/\u56fe\u7247-104-206x300.png 206w, http:\/\/222.128.65.89:18086\/wp-content\/uploads\/2024\/10\/\u56fe\u7247-104-686x1000.png 686w\" sizes=\"(max-width: 702px) 100vw, 702px\" \/><\/figure>\n\n\n\n<p>\u4e09\u3001CAN\u9a71\u52a8<\/p>\n\n\n\n<p>\u63a5\u6536autoware.universe\u7684\u63a7\u5236\u8bdd\u9898\uff0c\u5e76\u4e0b\u53d1\u5230\u5e95\u76d8\u63a7\u5236\u5b9e\u8f66\u8fd0\u52a8\uff0c\u540c\u65f6\u63a5\u6536\u5e95\u76d8\u53cd\u9988\u7684\u8f66\u7684\u901f\u5ea6\u4fe1\u606f\uff0c\u53d1\u9001\u7ed9Autoware\u8fdb\u884c\u4f4d\u59ff\u521d\u59cb\u5316\uff0c\u7f16\u5199\u4e86ROS2\u7248\u672c\u7684\u63a7\u5236\u5e95\u76d8\u7a0b\u5e8f\uff08CAN\u534f\u8bae\u4e0d\u540c\u4e0d\u80fd\u901a\u7528\uff0c\u53ea\u80fd\u4f5c\u4e3a\u53c2\u8003\uff09\uff1a<\/p>\n\n\n\n<pre class=\"wp-block-code\"><code># -*- coding: utf-8 -*-\nimport math\nimport time\nimport socket\nimport cantools\nimport threading\nimport rclpy\nfrom rclpy.node import Node\nfrom builtin_interfaces.msg import Time\nfrom binascii import hexlify\nfrom geometry_msgs.msg import TwistStamped, Twist\nfrom autoware_auto_control_msgs.msg import AckermannControlCommand\nfrom autoware_auto_vehicle_msgs.msg import ControlModeReport, GearReport, SteeringReport, VelocityReport\n\n# \u5f27\u5ea6\u8f6c\u89d2\u5ea6\u7cfb\u6570\nradian2angle = 57.29577951308232\n\n# \u521b\u5efasocket\u5957\u63a5\u5b57\nudp_socket = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)  # AF_INET\u8868\u793a\u4f7f\u7528ipv4,\u9ed8\u8ba4\u4e0d\u53d8\uff0cSOCK_DGRAM\u8868\u793a\u4f7f\u7528UDP\u901a\u4fe1\u534f\u8bae\n\n# \u7ed1\u5b9a\u7aef\u53e3port\nlocal_addr = (\"192.168.1.102\", 8882)  # \u672c\u5730ip\uff0c\u7aef\u53e3\u53f7\nudp_socket.bind(local_addr)  # \u7ed1\u5b9a\u7aef\u53e3\n\n# \u6307\u5b9a\u8981\u63a5\u6536\u7684\u524d\u4e94\u4e2a\u5b57\u8282\u7684CAN\u534f\u8bae\u6570\u636e\nEXPECTED_DATA = bytes(&#91;0x08, 0x00, 0x00, 0x01, 0x16])\n\n# \u6863\u4f4d+\u8f66\u901f0\u6307\u4ee4\uff1a08 00 00 00 E2 01 00 00 00 00 00 00 00\ndrive_by_wire_command = bytes(&#91;0x08, 0x00, 0x00, 0x00, 0xE2, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00])\n# \u63a7\u5236\u65b9\u5411\u76d8\u8f6c\u5230100\u5ea6\uff0c\u8f6c\u901f100\ntest1 = bytes(&#91;0x08, 0x00, 0x00, 0x04, 0x69, 0x20, 0x00, 0x00, 0x04, 0x64, 0x00, 0x64, 0x24])\n# \u63a7\u5236\u65b9\u5411\u76d8\u8f6c\u52300\u5ea6\uff0c\u8f6c\u901f50\ntest2 = bytes(&#91;0x08, 0x00, 0x00, 0x04, 0x69, 0x20, 0x00, 0x00, 0x04, 0x00, 0x00, 0x32, 0x16])\n\ndata_EV1 = {'Gear': 0, 'Invalid': 0, 'EV_Speed_L': 0, 'EV_Speed_H' : 0, \n            'Stay0': 0, 'Stay1': 0, 'Stay2': 0, 'Stay3': 0, 'Stay4': 0}\n\ndata_EPS2 = {'Work_mode': 32, 'Stay0': 0, 'Stay1': 0, 'Steer_Angle_H':0, \n             'Steer_Angle_L':0, 'Angle_alignment': 0, 'Angular_velocity': 100}\n\nmessage_EV1_front = bytes(&#91;0x08, 0x00, 0x00, 0x00, 0xE2]) # EV1\u7684\u5e27\u4fe1\u606f\u4e0e\u5e27ID\nmessage_EPS2_front = bytes(&#91;0x08, 0x00, 0x00, 0x04, 0x69]) # EPS2\u7684\u5e27\u4fe1\u606f\u4e0e\u5e27ID\n\n# \u8ba1\u7b97\u5f02\u6216\u6821\u9a8c\u503c\ndef Calculate_XOR_value(dict_data):\n    # \u63d0\u53d6\u6240\u6709\u503c\n    values = list(dict_data.values())\n\n    # \u8ba1\u7b97\u5f02\u6216\u6821\u9a8c\u7801\n    result = values&#91;0]\n    for value in values&#91;1:]:\n        result ^= value\n\n    # \u8fd4\u56de\n    return result\n\ndef calculate_speed(linear_speed):\n    EV_Speed_H = int((linear_speed * 185)) \/\/ 256\n    EV_Speed_L = int((linear_speed * 185)) % 256\n    # print('EV_Speed_H:%f' % EV_Speed_H)\n    # print('EV_Speed_L:%f' % EV_Speed_L)\n    data_EV1&#91;'EV_Speed_L'] = EV_Speed_L\n    data_EV1&#91;'EV_Speed_H'] = EV_Speed_H\n\ndef calculate_angle(linear_speed, angular_speed):\n    # \u8f6c\u5f2f\u7684\u89d2\u5ea6 = arctan(( \u89d2\u901f\u5ea6 \/ \u7ebf\u901f\u5ea6 ) * \u8f66\u957f ) \n    Steer_Angle = -math.atan((angular_speed\/linear_speed)*1) * radian2angle * 4.5\n    print('Steer_Angle:%f' % Steer_Angle)\n\n    # \u5b9e\u9645\u6d4b\u8bd5\u4e0b\u6765\uff0c\u65b9\u5411\u76d8\u8f6c\u52a8\u7684\u89d2\u5ea6\u8303\u56f4\u662f-27.5~27.5\uff0c\u5bf9\u5e94\u7684\u53d6\u503c\u8303\u56f4\u662f924~1124  27.5*3.6=99\n    Steer_Angle_H = int((Steer_Angle * 3.6) + 1024) \/\/ 256\n    Steer_Angle_L = int((Steer_Angle * 3.6) + 1024) % 256\n    # print('Steer_Angle_H:%f' % Steer_Angle_H)\n    # print('Steer_Angle_L:%f' % Steer_Angle_L)\n    data_EPS2&#91;'Steer_Angle_H'] = Steer_Angle_H\n    data_EPS2&#91;'Steer_Angle_L'] = Steer_Angle_L\n\ndef calculate_angle(angular_rad):\n    Steer_Angle = -angular_rad * radian2angle\n    print(\"Steer_Angle:\", Steer_Angle)\n    # \u5b9e\u9645\u6d4b\u8bd5\u4e0b\u6765\uff0c\u65b9\u5411\u76d8\u8f6c\u52a8\u7684\u89d2\u5ea6\u8303\u56f4\u662f-27.5~27.5\uff0c\u5bf9\u5e94\u7684\u53d6\u503c\u8303\u56f4\u662f924~1124  27.5*3.6=99\n    Steer_Angle_H = int((Steer_Angle * 3.6) + 1024) \/\/ 256\n    Steer_Angle_L = int((Steer_Angle * 3.6) + 1024) % 256\n    # print('Steer_Angle_H:%f' % Steer_Angle_H)\n    # print('Steer_Angle_L:%f' % Steer_Angle_L)\n    data_EPS2&#91;'Steer_Angle_H'] = Steer_Angle_H\n    data_EPS2&#91;'Steer_Angle_L'] = Steer_Angle_L\n\n# udp\u5411\u5e95\u76d8\u53d1\u9001can\u534f\u8bae\ndef udp_send_can(message_name):\n    udp_socket.sendto(message_name, (\"192.168.1.10\", 6666))\n\n# \u5904\u7406\u63a5\u6536\u5230\u7684CAN\u6d88\u606f\u7684\u51fd\u6570\ndef process_can_message(data,node):\n    # \u89e3\u7801CAN\u6d88\u606f\n    can_data = list(data&#91;5:])  # \u4ece\u7b2c6\u4e2a\u5b57\u8282\u5f00\u59cb\u662fCAN\u6570\u636e\n    message = node.db.decode_message(\"VCU\", can_data)\n\n    # \u6253\u5370\u89e3\u7801\u7ed3\u679c\n    # print(message)\n    # print('MP_AP:', message&#91;'MP_AP'])\n    # print('Gear:', message&#91;'Gear'])\n    # print('Driver_Break:', message&#91;'Driver_Break'])\n    # print('Steer_Angle_L:', message&#91;'Steer_Angle_L'])\n    # print('Steer_Angle_H:', message&#91;'Steer_Angle_H'])\n    # print('DM_Speed_L:', message&#91;'DM_Speed_L'])\n    # print('DM_Speed_H:', message&#91;'DM_Speed_H'])\n\n    # \u5904\u7406CAN\u4e2d\u63a5\u6536\u5230\u7684\u6570\u636e\uff0c\u8f6c\u5316\u6210\u7ebf\u901f\u5ea6\u548c\u89d2\u901f\u5ea6\n    feedback_twist_linear_x = (message&#91;'DM_Speed_H'] * 256 + message&#91;'DM_Speed_L']) \/ 185\n\n    Steer_Angle = (message&#91;'Steer_Angle_H'] * 256 + message&#91;'Steer_Angle_L'] - 1024) \/ 3.6\n    # \u89d2\u901f\u5ea6 = tan(\u8f6c\u5411\u89d2\u5ea6) * \u7ebf\u901f\u5ea6 \/ \u524d\u540e\u8f6e\u8f74\u8ddd\n    feedback_twist_angular_z = math.tan(Steer_Angle \/ radian2angle) * feedback_twist_linear_x \/ 1\n\n    if (message&#91;'Gear'] == 1):\n        feedback_twist_linear_x = feedback_twist_linear_x\n    elif (message&#91;'Gear'] == 2):\n        feedback_twist_linear_x = -feedback_twist_linear_x\n    else:\n        feedback_twist_linear_x = 0.0\n\n    # \u53d1\u5e03\u5904\u7406\u540e\u7684Twist\u6d88\u606f\u5230\u53e6\u4e00\u4e2a\u8bdd\u9898\n    node.publish_data(feedback_twist_linear_x, feedback_twist_angular_z)\n    node.pubilsh_control_mode(1)\n    node.pubilsh_gear(2)\n    node.pubilsh_steering(-Steer_Angle \/ radian2angle)\n    node.pubilsh_velocity(\"base_link\", feedback_twist_linear_x, 0.0, 0.0)\n\n# \u63a5\u6536\u6570\u636e\u7684\u7ebf\u7a0b\u51fd\u6570\ndef receive_data(node):\n    while rclpy.ok():\n        data, addr = udp_socket.recvfrom(13)\n        # print(hexlify(data).decode('ascii'))\n\n        # \u786e\u4fdd\u63a5\u6536\u5230\u7684\u6570\u636e\u6ee1\u8db3\u9884\u671f\u7684CAN\u6570\u636e\n        if data&#91;:5] == EXPECTED_DATA:\n            # \u5728\u65b0\u7684\u7ebf\u7a0b\u4e2d\u5904\u7406CAN\u6d88\u606f\uff0c\u4ee5\u4fdd\u8bc1\u5b9e\u65f6\u6027\n            threading.Thread(target=process_can_message, args=(data,node)).start()\n\n\nclass TopicSubscriberPublisher(Node):\n    def __init__(self):\n        super().__init__('cmd_vel_to_can')\n        # \u52a0\u8f7ddbc\u6587\u4ef6\n        self.declare_parameter(\"dbc\")\n        dbcfile = self.get_parameter(\"dbc\").get_parameter_value().string_value\n        self.db = cantools.database.load_file(dbcfile)\n\n        self.subscriber = self.create_subscription(AckermannControlCommand, 'control\/command\/control_cmd', self.sub_callback, 10)\n        # self.publisher = self.create_publisher(Twist, 'twist_cmd_feedback', self.pub_callback, 10)\n        self.publisher_data = self.create_publisher(Twist, 'twist_cmd_feedback', 10)\n        self.publisher_control_mode = self.create_publisher(ControlModeReport, '\/vehicle\/status\/control_mode', 10)\n        self.publisher_gear = self.create_publisher(GearReport, '\/vehicle\/status\/gear_status', 10)\n        self.publisher_steering = self.create_publisher(SteeringReport, '\/vehicle\/status\/steering_status', 10)\n        self.publisher_velocity = self.create_publisher(VelocityReport, '\/vehicle\/status\/velocity_status', 10)\n        # self.get_logger().info('TopicSubscriberPublisher node initialized')\n\n    def sub_callback(self, msg):\n        # 1. \u53d1\u9001\u6863\u4f4d+\u8f66\u901f0\u6307\u4ee4\uff1a08 00 00 00 E2 01 00 00 00 00 00 00 00\n        udp_send_can(drive_by_wire_command)\n\n        # 2. \u63a5\u6536autoware\u4f20\u6765\u7684\u7ebf\u901f\u5ea6\u548c\u89d2\u901f\u5ea6\n        EV_Speed = msg.longitudinal.speed\n        # angular_velocity = msg.lateral.steering_tire_rotation_rate\n        angular_rad = msg.lateral.steering_tire_angle\n        # print('EV_Speed:%f' % EV_Speed)\n        # print('angular_velocity:%f' % angular_velocity)\n        print('angular_rad:%f' % angular_rad)\n\n        # 3. \u8ba1\u7b97\u6863\u4f4d\u3001\u901f\u5ea6\u3001\u89d2\u5ea6\n        if (EV_Speed > 0):\n            data_EV1&#91;'Gear'] = 1\n            calculate_speed(EV_Speed)\n            # calculate_angle(EV_Speed, angular_velocity)\n            calculate_angle(angular_rad)\n\n        elif (EV_Speed &lt; 0):\n            data_EV1&#91;'Gear'] = 2\n            calculate_speed(-EV_Speed)\n            # calculate_angle(-EV_Speed, angular_velocity)\n            calculate_angle(angular_rad)\n\n        else:\n            data_EV1&#91;'Gear'] = 0\n            calculate_speed(0)\n            # calculate_angle(1, angular_velocity)\n            calculate_angle(angular_rad)\n\n        # 4. \u53d1\u9001can\u6d88\u606f\n        message_EV1 = self.db.encode_message(\"EV1\", data_EV1)\n        message_linear_velocity = message_EV1_front + message_EV1\n        # print(hexlify(message_linear_velocity).decode('ascii'))\n        udp_send_can(message_linear_velocity)\n\n        # \u8ba1\u7b97\u5f02\u6216\u6821\u9a8c\u7801\n        Check = Calculate_XOR_value(data_EPS2)\n        data_EPS2.update({'Check' : Check})\n        message_EPS2 = self.db.encode_message(\"EPS2\", data_EPS2)\n        message_angle = message_EPS2_front + message_EPS2\n        # print(hexlify(message_angle).decode('ascii'))\n        udp_send_can(message_angle)\n\n    def publish_data(self, data1, data2):\n        msg = Twist()\n        msg.linear.x = data1\n        msg.angular.z = data2\n        self.publisher_data.publish(msg)\n    \n    def pubilsh_control_mode(self, data):\n        msg = ControlModeReport()\n        msg.mode = data\n        self.publisher_control_mode.publish(msg)\n\n    def pubilsh_gear(self, data):\n        msg = GearReport()\n        msg.report = data\n        self.publisher_gear.publish(msg)\n\n    def pubilsh_steering(self, data):\n        msg = SteeringReport()\n        msg.steering_tire_angle = data\n        self.publisher_steering.publish(msg)\n\n    def pubilsh_velocity(self, data1, data2, data3, data4):\n        msg = VelocityReport()\n        # \u83b7\u53d6\u5f53\u524d\u65f6\u95f4\n        # \u79d2\n        sec_ = int(time.time())\n        # \u7eb3\u79d2\n        nanosec_ = int((time.time()-sec_)*1e9)\n        msg.header.stamp = Time(sec = sec_,nanosec = nanosec_)\n        msg.header.frame_id = data1\n        msg.longitudinal_velocity = data2\n        msg.lateral_velocity = data3\n        msg.heading_rate = data4\n        self.publisher_velocity.publish(msg)\n\ndef main():\n    # \u521d\u59cb\u5316\n    rclpy.init()\n\n    # \u65b0\u5efa\u4e00\u4e2a\u8282\u70b9\n    node = TopicSubscriberPublisher()\n\n    # \u542f\u52a8\u63a5\u6536CAN\u6570\u636e\u7684\u7ebf\u7a0b\n    threading.Thread(target=receive_data, args=(node,)).start()\n\n    # \u4fdd\u6301\u8282\u70b9\u8fd0\u884c\uff0c\u68c0\u6d4b\u662f\u5426\u6536\u5230\u9000\u51fa\u6307\u4ee4\uff08Ctrl+C\uff09\n    rclpy.spin(node)\n\n    # \u6e05\u7406\u5e76\u5173\u95edros2\u8282\u70b9\n    node.destroy_node()\n    rclpy.shutdown()\n\nif __name__ == '__main__':\n    main()\n<\/code><\/pre>\n\n\n\n<p>\u7f16\u5199setup.py\u548claunch\u6587\u4ef6<\/p>\n\n\n\n<pre class=\"wp-block-code\"><code>from setuptools import setup\n\npackage_name = 'can_ros2_bridge'\n\nsetup(\n    name=package_name,\n    version='0.0.0',\n    packages=&#91;package_name],\n    # \u5b89\u88c5\u6587\u4ef6\n    data_files=&#91;\n        ('share\/ament_index\/resource_index\/packages',\n            &#91;'resource\/' + package_name]),\n        ('share\/' + package_name, &#91;'package.xml']),\n        ('share\/' +package_name, &#91;'launch\/can.launch.py']),\n        ('share\/' +package_name, &#91;'config\/CarCAN.dbc']),\n    ],\n    install_requires=&#91;'setuptools'],\n    zip_safe=True,\n    maintainer='car',\n    maintainer_email='car@todo.todo',\n    description='TODO: Package description',\n    license='TODO: License declaration',\n    tests_require=&#91;'pytest'],\n    # \u53ef\u6267\u884c\u6587\u4ef6\n    entry_points={\n        'console_scripts': &#91;\n            'cmd_vel_to_can = can_ros2_bridge.send_message:main',\n        ],\n    },\n)\n<\/code><\/pre>\n\n\n\n<p>\u4fee\u6539\u9759\u6001IP\uff08\u6ce8\u610f\u5173\u6389WIFI\uff09\uff0c\u542f\u52a8CAN\uff0c\u80fd\u6210\u529f\u63a7\u5236\u5e95\u76d8<\/p>\n\n\n\n<p>\u56db\u3001\u76f8\u673a\u9a71\u52a8<\/p>\n\n\n\n<p><a><\/a>4.1 \u5b89\u88c5\u9a71\u52a8<\/p>\n\n\n\n<p>\u672c\u6587\u4f7f\u7528\u7684\u76f8\u673a\u6ca1\u6709\u5b98\u65b9\u9a71\u52a8\uff0c\u91c7\u7528\u7684\u76f8\u673a\u9a71\u52a8\u6e90\u7801\u5730\u5740\uff1a<a href=\"https:\/\/github.com\/ros-drivers\/usb_cam\/tree\/ros2\">https:\/\/github.com\/ros-drivers\/usb_cam\/tree\/ros2<\/a>\uff0c\u5c06\u4ee3\u7801\u4e0b\u8f7d\u4e0b\u6765\u653e\u5230\u5de5\u4f5c\u7a7a\u95f4src\u7f16\u8bd1\u8fd0\u884c\uff1a<\/p>\n\n\n\n<pre class=\"wp-block-code\"><code>colcon build\nsource install\/setup.bash\nros2 run usb_cam usb_cam_node_exe --ros-args --params-file src\/usb_cam-ros2\/config\/params_1.yaml\n# \u6216\u8005\nros2 launch usb_cam camera.launch.py #\u4f46\u662f\u8fd9\u4e2a\u8fd0\u884c\u53ef\u80fd\u6709\u4e00\u4e9b\u95ee\u9898\uff0c\u6211\u4eec\u5728\u4e0b\u4e00\u8282\u91cd\u65b0\u5199\u4e00\u4e2a\n<\/code><\/pre>\n\n\n\n<p>\u518d\u6253\u5f00\u4e00\u4e2a\u8282\u70b9\uff0c\u663e\u793a\u56fe\u50cf\uff1a<\/p>\n\n\n\n<pre class=\"wp-block-code\"><code>ros2 run usb_cam show_image.py\n<\/code><\/pre>\n\n\n\n<p>\u5982\u679c\u662f\u5916\u7f6e\u7684\u76f8\u673a\uff0c\u6b64\u65f6\u5927\u6982\u7387\u65e0\u6cd5\u9a71\u52a8\uff0c\u56e0\u4e3a\u76f8\u673a\u9ed8\u8ba4\u6302\u8f7d\u70b9\u662f\/dev\/video0\uff08\u4f46\u5b83\u4e00\u822c\u662f\u7535\u8111\u81ea\u5e26\u7684\u6444\u50cf\u5934\uff09\uff0c\u67e5\u770b\u76f8\u673a\u6302\u8f7d\u5730\u5740\uff1a<\/p>\n\n\n\n<pre class=\"wp-block-code\"><code>ls \/dev\/\n<\/code><\/pre>\n\n\n\n<figure class=\"wp-block-image size-large\"><img loading=\"lazy\" decoding=\"async\" width=\"1024\" height=\"548\" src=\"http:\/\/192.168.10.115\/wp-content\/uploads\/2024\/10\/\u56fe\u7247-105-1024x548.png\" alt=\"\" class=\"wp-image-16775\" srcset=\"http:\/\/222.128.65.89:18086\/wp-content\/uploads\/2024\/10\/\u56fe\u7247-105-1024x548.png 1024w, http:\/\/222.128.65.89:18086\/wp-content\/uploads\/2024\/10\/\u56fe\u7247-105-300x161.png 300w, http:\/\/222.128.65.89:18086\/wp-content\/uploads\/2024\/10\/\u56fe\u7247-105-768x411.png 768w, http:\/\/222.128.65.89:18086\/wp-content\/uploads\/2024\/10\/\u56fe\u7247-105.png 1452w\" sizes=\"(max-width: 1024px) 100vw, 1024px\" \/><\/figure>\n\n\n\n<p>\u53ef\u4ee5\u901a\u8fc7\u63d2\u62d4\u76f8\u673a\u5224\u65ad\u6302\u8f7d\u5730\u5740\uff0c\u6211\u4eec\u662f\/dev\/video2\uff0c\u5728\u53c2\u6570\u6587\u4ef6\u4e2d\u4fee\u6539video_device\u4e3a\/dev\/video2\uff0c\u518d\u6b21\u9a71\u52a8\u5373\u53ef\u770b\u5230\u5916\u7f6e\u76f8\u673a\u7684\u56fe\u50cf<\/p>\n\n\n\n<figure class=\"wp-block-image size-full\"><img loading=\"lazy\" decoding=\"async\" width=\"821\" height=\"414\" src=\"http:\/\/192.168.10.115\/wp-content\/uploads\/2024\/10\/\u56fe\u7247-106.png\" alt=\"\" class=\"wp-image-16776\" srcset=\"http:\/\/222.128.65.89:18086\/wp-content\/uploads\/2024\/10\/\u56fe\u7247-106.png 821w, http:\/\/222.128.65.89:18086\/wp-content\/uploads\/2024\/10\/\u56fe\u7247-106-300x151.png 300w, http:\/\/222.128.65.89:18086\/wp-content\/uploads\/2024\/10\/\u56fe\u7247-106-768x387.png 768w\" sizes=\"(max-width: 821px) 100vw, 821px\" \/><\/figure>\n\n\n\n<p>\u7136\u540e\u518d\u6b21\u8fd0\u884c\uff0c\u53ef\u4ee5\u9a71\u52a8\u76f8\u673a\u4e86\uff0c\u76f8\u673a\u8bdd\u9898\u4e3a\/image_raw<\/p>\n\n\n\n<h3 class=\"wp-block-heading\">4.2 \u4fee\u6539\u76f8\u673a\u53c2\u6570<\/h3>\n\n\n\n<p>\u4e0a\u9762\u7b80\u5355\u7684\u8fd0\u884c\u5b9e\u9645\u4e0a\u53ef\u80fd\u65e0\u6cd5\u9002\u914d\u4f60\u7684\u76f8\u673a\uff08\u53ef\u4ee5\u9a71\u52a8\u4f46\u662f\u6548\u679c\u5f88\u5dee\uff09\uff0c\u6211\u4eec\u9700\u8981\u4fee\u6539\u53c2\u6570\uff0c\u65b0\u5efa\u4e00\u4e2a\u53c2\u6570\u6587\u4ef6\uff08\u4f8b\u5982config\/JR_HF868.yaml\uff09\uff0c\u5185\u5bb9\u5982\u4e0b\uff1a<\/p>\n\n\n\n<pre class=\"wp-block-code\"><code>\/**:\n    ros__parameters:\n      # \u8bbe\u5907\u6302\u8f7d\u70b9\n      video_device: \"\/dev\/video2\"\n      # \u5e27\u7387\n      framerate: 30.0\n      io_method: \"mmap\"\n      # \u5750\u6807\u7cfb\n      frame_id: \"camera\"\n      # \u50cf\u7d20\u683c\u5f0f\n      pixel_format: \"mjpeg2rgb\"  # see usb_cam\/supported_formats for list of supported formats\n      # \u50cf\u7d20\u5c3a\u5bf8\n      image_width: 1920\n      image_height: 1080\n      # \u76f8\u673a\u540d\u79f0\n      camera_name: \"JR_HF868\"\n      # \u6807\u5b9a\u53c2\u6570\n      camera_info_url: \"package:\/\/usb_cam\/config\/camera_info.yaml\"\n      # \u4eae\u5ea6\n      brightness: 50\n      # \u5bf9\u6bd4\u5ea6\n      contrast: 50\n      # \u9971\u548c\u5ea6\n      saturation: 50\n      # \u6e05\u6670\u5ea6\n      sharpness: 80\n      # \u589e\u76ca\n      gain: -1\n      # \u767d\u5e73\u8861\n      auto_white_balance: true\n      white_balance: 4000\n      # \u66dd\u5149\n      autoexposure: true\n      exposure: 100\n      # \u805a\u7126\n      autofocus: false\n      focus: -1\n<\/code><\/pre>\n\n\n\n<p>\u5176\u4e2d\u6709\u51e0\u70b9\u9700\u8981\u6ce8\u610f\u7684\uff1a<br>\uff081\uff09\u5c06\u76f8\u673a\u5206\u8fa8\u7387\u4fee\u6539\u4e3a1920*1080\uff08\u6216\u8005\u4f60\u7684\u76f8\u673a\u652f\u6301\u7684\uff09\uff1b<br>\uff082\uff09\u5c06\u8bbe\u5907\u6302\u8f7d\u70b9\u6539\u6210\/dev\/video2\uff08\u6216\u8005\u81ea\u5df1\u67e5\u5230\u7684\uff09\uff1b<br>\uff083\uff09\u9ed8\u8ba4\u7684pixel_format\uff0c\u5f53\u76f8\u673a\u8fd0\u52a8\u8fc7\u5feb\uff0c\u56fe\u7247\u7684\u8fd0\u52a8\u7578\u53d8\u6bd4\u8f83\u5927\uff0c\u53d1\u73b0\u5728\u8fd0\u884c\u76f8\u673a\u8282\u70b9\u7684\u65f6\u5019\uff0c\u4f1a\u6253\u5370\u51fa\u76f8\u673a\u652f\u6301\u7684\u4e00\u4e9b\u53c2\u6570\uff1a<br><\/p>\n\n\n\n<figure class=\"wp-block-image size-large\"><img loading=\"lazy\" decoding=\"async\" width=\"1024\" height=\"410\" src=\"http:\/\/192.168.10.115\/wp-content\/uploads\/2024\/10\/\u56fe\u7247-107-1024x410.png\" alt=\"\" class=\"wp-image-16777\" srcset=\"http:\/\/222.128.65.89:18086\/wp-content\/uploads\/2024\/10\/\u56fe\u7247-107-1024x410.png 1024w, http:\/\/222.128.65.89:18086\/wp-content\/uploads\/2024\/10\/\u56fe\u7247-107-300x120.png 300w, http:\/\/222.128.65.89:18086\/wp-content\/uploads\/2024\/10\/\u56fe\u7247-107-768x308.png 768w, http:\/\/222.128.65.89:18086\/wp-content\/uploads\/2024\/10\/\u56fe\u7247-107.png 1073w\" sizes=\"(max-width: 1024px) 100vw, 1024px\" \/><\/figure>\n\n\n\n<p>\u6211\u4eec\u7684\u76f8\u673a\u5728YUYV 4:2:2: 1920 x 1080\u8fd9\u4e2a\u53c2\u6570\u4e0b\u53ea\u652f\u63016 Hz\u7684\u5e27\u7387\uff0c\u76f8\u673a\u5728Motion-JPEG: 1920 x 1080\u8fd9\u4e2a\u53c2\u6570\u4e0b\u652f\u630130 Hz\u7684\u5e27\u7387\uff0c\u67e5\u627esenser_driver\/src\/usb_cam-ros2\/include\/usb_cam\/usb_cam.hpp\u6587\u4ef6\uff0c\u53ef\u4ee5\u627e\u5230\u9a71\u52a8\u652f\u6301\u7684\u50cf\u7d20\u683c\u5f0f\uff0c\u6709\u5982\u4e0b\u51e0\u79cd<br><\/p>\n\n\n\n<figure class=\"wp-block-image size-large\"><img loading=\"lazy\" decoding=\"async\" width=\"1024\" height=\"660\" src=\"http:\/\/192.168.10.115\/wp-content\/uploads\/2024\/10\/\u56fe\u7247-108-1024x660.png\" alt=\"\" class=\"wp-image-16778\" srcset=\"http:\/\/222.128.65.89:18086\/wp-content\/uploads\/2024\/10\/\u56fe\u7247-108-1024x660.png 1024w, http:\/\/222.128.65.89:18086\/wp-content\/uploads\/2024\/10\/\u56fe\u7247-108-300x193.png 300w, http:\/\/222.128.65.89:18086\/wp-content\/uploads\/2024\/10\/\u56fe\u7247-108-768x495.png 768w, http:\/\/222.128.65.89:18086\/wp-content\/uploads\/2024\/10\/\u56fe\u7247-108.png 1140w\" sizes=\"(max-width: 1024px) 100vw, 1024px\" \/><\/figure>\n\n\n\n<p>\u4fee\u6539pixel_format\u53c2\u6570\uff0c\u6539\u6210mjpeg2rgb<br>\uff084\uff09\u4fee\u6539\u4eae\u5ea6\uff0c\u5bf9\u6bd4\u5ea6\uff0c\u9971\u548c\u5ea6\u7b49\u53c2\u6570<br>\u65b0\u5199\u4e00\u4e2a\u542f\u52a8\u6587\u4ef6\uff08launch\/JR_HF868.launch.py\uff09\uff1a<\/p>\n\n\n\n<pre class=\"wp-block-code\"><code>import os\n\nfrom ament_index_python.packages import get_package_share_directory\nfrom launch import LaunchDescription\nfrom launch_ros.actions import Node\n \ndef generate_launch_description():\n   config = os.path.join(\n      get_package_share_directory('usb_cam'),\n      'config',\n      'JR_HF868.yaml'\n      )\n   return LaunchDescription(&#91;\n      Node(\n         package='usb_cam',\n         executable='usb_cam_node_exe',\n         name='usb_cam_node_exe',\n         parameters=&#91;config]\n      ),\n   ])\n<\/code><\/pre>\n\n\n\n<p>\u7136\u540e\u518d\u91cd\u65b0\u7f16\u8bd1\uff0c\u8fd0\u884c\u8282\u70b9\uff0c\u73b0\u5728\u76f8\u673a\u7684\u56fe\u50cf\u50cf\u7d20\u6bd4\u8f83\u9ad8\uff0c\u800c\u4e14\u5feb\u901f\u8fd0\u52a8\u7684\u65f6\u5019\u7578\u53d8\u4e5f\u5c0f\uff1a<\/p>\n\n\n\n<pre class=\"wp-block-code\"><code>source install\/setup.bash\nros2 launch usb_cam JR_HF868.launch.py\n<\/code><\/pre>\n\n\n\n<p>\u4e94\u3001GNSS\u9a71\u52a8<\/p>\n\n\n\n<p>GNSS\u662f\u53ef\u9009\u7684\uff0c\u8fd9\u91cc\u4f7f\u7528\u7684\u662f\u534e\u6d4bM620RTK\u6a21\u5757\u9a71\u52a8\u3002<br>\u7531\u4e8eROS2\u6ca1\u6709\u518d\u5c01\u88c5\u4e32\u53e3\u5e93serial\uff0c\u56e0\u6b64\u9700\u8981\u624b\u52a8\u5b89\u88c5serial\uff1a<\/p>\n\n\n\n<pre class=\"wp-block-code\"><code>git clone https:\/\/github.com\/ZhaoXiangBox\/serial\ncd serial &amp;&amp; mkdir build\ncmake .. &amp;&amp; make\nsudo make install\n<\/code><\/pre>\n\n\n\n<p>Cmake\u914d\u7f6e\uff1a<\/p>\n\n\n\n<pre class=\"wp-block-code\"><code>set(CMAKE_INSTALL_RPATH \/usr\/local\/lib)\nfind_package(serial REQUIRED)\nament_target_dependencies(exe \"serial\")\n<\/code><\/pre>\n\n\n\n<p>\u63a5\u4e0b\u6765\u7f16\u5199\u4e32\u53e3\u901a\u4fe1\uff0c\u8bfb\u53d6GNSS\u6570\u636e\uff08\u6839\u636eCHCCGI610\u7684ROS1\u4ee3\u7801\u4fee\u6539\u800c\u6765\uff09<\/p>\n\n\n\n<pre class=\"wp-block-code\"><code>#include \"rclcpp\/rclcpp.hpp\"\n#include \"std_msgs\/msg\/string.hpp\"\n#include \"sensor_msgs\/msg\/nav_sat_fix.hpp\"\n#include &lt;serial\/serial.h>\n\n\/\/ $GPGGA\n\/\/ \u4f8b\uff1a$GPGGA,073217.00,3109.93434012,N,12117.26502692,E,4,48,0.59,25.441,M,11.090,M,2.8,0000*4F\n\/\/ \u5b57\u6bb50\uff1a$GPGGA\uff0c\u8bed\u53e5ID\uff0c\u8868\u660e\u8be5\u8bed\u53e5\u4e3aGlobal Positioning System Fix Data\uff08GGA\uff09GPS\u5b9a\u4f4d\u4fe1\u606f\n\/\/ \u5b57\u6bb51\uff1aUTC \u65f6\u95f4\uff0chhmmss.sss\uff0c\u65f6\u5206\u79d2\u683c\u5f0f\n\/\/ \u5b57\u6bb52\uff1a\u7eac\u5ea6ddmm.mmmm\uff0c\u5ea6\u5206\u683c\u5f0f\uff08\u524d\u5bfc\u4f4d\u6570\u4e0d\u8db3\u5219\u88650\uff09\n\/\/ \u5b57\u6bb53\uff1a\u7eac\u5ea6N\uff08\u5317\u7eac\uff09\u6216S\uff08\u5357\u7eac\uff09\n\/\/ \u5b57\u6bb54\uff1a\u7ecf\u5ea6dddmm.mmmm\uff0c\u5ea6\u5206\u683c\u5f0f\uff08\u524d\u5bfc\u4f4d\u6570\u4e0d\u8db3\u5219\u88650\uff09\n\/\/ \u5b57\u6bb55\uff1a\u7ecf\u5ea6E\uff08\u4e1c\u7ecf\uff09\u6216W\uff08\u897f\u7ecf\uff09\n\/\/ \u5b57\u6bb56\uff1aGPS\u72b6\u6001\uff0c0=\u672a\u5b9a\u4f4d\uff0c1=\u5355\u70b9\u5b9a\u4f4d\uff0c2=\u4f2a\u8ddd\/SBAS\uff0c3=\u65e0\u6548PPS\uff0c4=RTK\u56fa\u5b9a\uff0c5=RTK\u6d6e\u52a8\n\/\/ \u5b57\u6bb57\uff1a\u6b63\u5728\u4f7f\u7528\u7684\u536b\u661f\u6570\u91cf\n\/\/ \u5b57\u6bb58\uff1aHDOP\u6c34\u5e73\u7cbe\u5ea6\u56e0\u5b50\uff080.5 - 99.9\uff09\n\/\/ \u5b57\u6bb59\uff1a\u6d77\u62d4\u9ad8\u5ea6\uff08-9999.9 \u2014 +99999.9\uff09\n\/\/ \u5b57\u6bb510\uff1aM\u4e3a\u5355\u4f4d\u7c73\n\/\/ \u5b57\u6bb511\uff1a\u5730\u7403\u692d\u7403\u9762\u76f8\u5bf9\u5927\u5730\u6c34\u51c6\u9762\u7684\u9ad8\u5ea6\n\/\/ \u5b57\u6bb512\uff1aM\u4e3a\u5355\u4f4d\u7c73\n\/\/ \u5b57\u6bb513\uff1a\u5dee\u5206\u65f6\u95f4\uff08\u4ece\u6700\u8fd1\u4e00\u6b21\u63a5\u6536\u5230\u5dee\u5206\u4fe1\u53f7\u5f00\u59cb\u7684\u79d2\u6570\uff0c\u5982\u679c\u4e0d\u662f\u5dee\u5206\u5b9a\u4f4d\u5c06\u4e3a\u7a7a\uff09\n\/\/ \u5b57\u6bb514\uff1a\u5dee\u5206\u7ad9ID\u53f70000 - 1023\uff08\u524d\u5bfc\u4f4d\u6570\u4e0d\u8db3\u5219\u88650\uff0c\u5982\u679c\u4e0d\u662f\u5dee\u5206\u5b9a\u4f4d\u5c06\u4e3a\u7a7a\uff09*\u6821\u9a8c\u503c\n\nclass GNSSPublisher : public rclcpp::Node\n{\npublic:\n    GNSSPublisher(const char *nodeName) : Node(nodeName),\n                                          StateParser(0), CntByte(0), CntDelimiter(0), tmpint(0)\n    {\n        port_name = this->declare_parameter(\"~port_name\", \"\/dev\/ttyUSB0\");\n        PosDelimiter&#91;15] = {0};\n        temp_field&#91;30] = {0};\n        gnss_pub_ = this->create_publisher&lt;sensor_msgs::msg::NavSatFix>(\"\/sensing\/gnss\/ublox\/nav_sat_fix\", 10);\n\n        try\n        {\n            \/\/ \u8bbe\u7f6e\u4e32\u53e3\u5c5e\u6027\uff0c\u5e76\u6253\u5f00\u4e32\u53e3\n            ser.setPort(port_name);\n            ser.setBaudrate(460800);\n            serial::Timeout to = serial::Timeout::simpleTimeout(1000); \/\/ \u8d85\u65f6\u5b9a\u4e49\uff0c\u5355\u4f4d\uff1ams\n            ser.setTimeout(to);\n            ser.open();\n        }\n        catch (serial::IOException &amp;e)\n        {\n            std::cout &lt;&lt; port_name + \" open failed, please check the permission of port ,run command \\\"sudo chmod 777 \" + port_name + \"\\\" and try again\uff01\" &lt;&lt; std::endl;\n            getchar();\n            rclcpp::shutdown();\n        }\n\n        \/\/ \u68c0\u6d4b\u4e32\u53e3\u662f\u5426\u5df2\u7ecf\u6253\u5f00\uff0c\u5e76\u7ed9\u51fa\u63d0\u793a\u4fe1\u606f\n        if (ser.isOpen())\n        {\n            std::cout &lt;&lt; port_name + \" open successed!\" &lt;&lt; std::endl;\n        }\n        else\n        {\n            std::cout &lt;&lt; port_name + \" open failed!\" &lt;&lt; std::endl;\n            getchar();\n            rclcpp::shutdown();\n        }\n\n        rclcpp::Rate loop_rate(100);           \/\/ \u8bbe\u7f6e\u5faa\u73af\u9891\u7387\u4e3a100Hz\n        ser.flushInput();                      \/\/ \u5728\u5f00\u59cb\u6b63\u5f0f\u63a5\u6536\u6570\u636e\u524d\u5148\u6e05\u9664\u4e32\u53e3\u7684\u63a5\u6536\u7f13\u51b2\u533a\n        memset(OneFrame, 0, sizeof(OneFrame)); \/\/ \u6e05\u7a7agps\u5b57\u7b26\u4e32\n        int framecnt = 0;\n        CntByte = 0; \/\/ \u6307\u5411OneFrame\u7684\u7b2c\u4e00\u4e2a\u4f4d\u7f6e\n\n        while (rclcpp::ok())\n        {\n            int i, j;\n            int start; \/\/ \u5f53\u524d\u4f4d\u7f6e\n            int pos;   \/\/ \u4e0b\u4e00\u4e2a\u5206\u9694\u7b26\u7684\u4f4d\u7f6e\n            int numinbuf;\n            int numgetted;\n            auto gnss_msg = std::make_unique&lt;sensor_msgs::msg::NavSatFix>();\n\n            try\n            {\n                numinbuf = ser.available(); \/\/ available()\u8fd4\u56de\u4ece\u4e32\u53e3\u7f13\u51b2\u533a\u8bfb\u56de\u7684\u5b57\u7b26\u6570\n                                            \/\/ std::cout&lt;&lt;\"\u4e32\u53e3\u7f13\u51b2\u533a\u7684\u6570\u636e\u6709\"&lt;&lt;numinbuf&lt;&lt;\"\u4e2a\"&lt;&lt;std::endl;\n                                            \/\/ initrd.img.oldCLEAR();\n                                            \/\/ printf(\"bytes in buf = %d\\n\",numinbuf);\n            }\n            catch (serial::IOException &amp;e)\n            {\n                std::cout &lt;&lt; \"Port crashed! Please check cable!\" &lt;&lt; std::endl;\n                getchar();\n                rclcpp::shutdown();\n            }\n\n            if (numinbuf > 0) \/\/ \u4e32\u53e3\u7f13\u51b2\u533a\u6709\u6570\u636e\n            {\n                numgetted = ser.read(rbuf, numinbuf); \/\/ \u4e32\u53e3\u7f13\u51b2\u533a\u6570\u636e\u8bfb\u5230rbuf\u4e2d\n                if (numgetted == numinbuf)            \/\/ \u53d6\u56de\u7684\u6570\u636e\u4e2a\u6570\u4e0e\u7f13\u51b2\u533a\u4e2d\u6709\u7684\u6570\u636e\u4e2a\u6570\u76f8\u540c\uff0c\u8bf4\u660e\u8bfb\u4e32\u53e3\u6210\u529f\n                {\n                    for (int i = 0; i &lt; numgetted; i++) \/\/ \u5bf9\u6536\u5230\u7684\u5b57\u7b26\u9010\u4e2a\u5904\u7406\n                    {\n                        \/\/ \u5728\u4e00\u5e27\u6570\u636e\u7684\u63a5\u6536\u8fc7\u7a0b\u4e2d\uff0c\u53ea\u8981\u9047\u5230\u975e$GPCHC\u5e27\u5934\u5c31\u91cd\u65b0\u5f00\u59cb\n                        \/\/ \u6b64\u5904\u7406\u5177\u6709\u6700\u9ad8\u4f18\u5148\u7ea7\uff0c\u4f1a\u91cd\u7f6e\u72b6\u6001\u673a\n                        if (rbuf&#91;i] == '$' &amp;&amp; rbuf&#91;i + 3] != 'G' &amp;&amp; rbuf&#91;i + 4] != 'G' &amp;&amp; rbuf&#91;i + 5] != 'A')\n                        {\n                            memset(OneFrame, 0, sizeof(OneFrame));\n                            StateParser = 0;\n                            break; \/\/ \u4e2d\u65ad\u5faa\u73af\n                        }\n                        \/\/ \u6b63\u5e38\u5904\u7406\u8fc7\u7a0b\n                        switch (StateParser)\n                        {\n                        \/\/ \u7b49\u5f85\u8bed\u53e5\u5f00\u59cb\u6807\u5fd7'$'\n                        case 0:\n                            if (rbuf&#91;i] == '$' &amp;&amp; rbuf&#91;i + 3] == 'G' &amp;&amp; rbuf&#91;i + 4] == 'G' &amp;&amp; rbuf&#91;i + 5] == 'A') \/\/ \u6536\u5230\u8bed\u53e5\u5f00\u59cb\u6807\u5fd7\n                            {\n                                memset(OneFrame, 0, sizeof(OneFrame));\n                                OneFrame&#91;0] = '$';\n                                CntByte = 1; \/\/ \u5f00\u59cb\u5bf9\u5e27\u957f\u5ea6\u7684\u8ba1\u6570\n                                StateParser = 1;\n                            }\n                            break;\n\n                        \/\/ \u5bfb\u627e\u5e27\u5934\"$GPGGA,\"\n                        case 1:\n                            OneFrame&#91;CntByte] = rbuf&#91;i];\n                            CntByte++; \/\/ \u6307\u5411\u4e0b\u4e00\u4e2a\u7a7a\u4f4d\n\n                            if (rbuf&#91;i] == ',')\n                            {\n                                if (strncmp(OneFrame, \"$GPGGA,\", 7) == 0)\n                                {\n                                    CntDelimiter = 0;              \/\/ \u5206\u9694\u7b26\u8ba1\u6570\u4ece0\u5f00\u59cb\n                                    PosDelimiter&#91;0] = CntByte - 1; \/\/ \u8bb0\u5f55\u5206\u9694\u7b26\u5728OneFrame\u4e2d\u7684\u4f4d\u7f6e\n                                    \/\/ std::cout&lt;&lt;\"PosDelimiter&#91;0]\"&lt;&lt;PosDelimiter&#91;0]&lt;&lt;std::endl;\n                                    StateParser = 2;\n                                    \/\/ std::cout&lt;&lt;\"\u5bfb\u627e\u5e27\u5934$GPGGA\u5b8c\u6210\"&lt;&lt;std::endl;\n                                }\n                                else \/\/ \u5e27\u5934\u9519\u8bef\n                                {\n                                    memset(OneFrame, 0, sizeof(OneFrame));\n                                    StateParser = 0;\n                                    \/\/ std::cout&lt;&lt;\"\u5bfb\u627e\u5e27\u5934$GPGGA\u5931\u8d25\"&lt;&lt;std::endl;\n                                }\n                            }\n                            break;\n\n                        \/\/ \u63a5\u6536\u5404\u6570\u636e\u57df\n                        case 2:\n                            \/\/ std::cout&lt;&lt;\"\u5f00\u59cb\u63a5\u53d7\u5404\u4e2a\u6570\u636e\u57df\"&lt;&lt;std::endl;\n                            OneFrame&#91;CntByte] = rbuf&#91;i];\n                            \/\/ std::cout&lt;&lt;\"\u63a5\u53d7\u5b57\u7b26\"&lt;&lt;rbuf&#91;i]&lt;&lt;std::endl;\n                            CntByte++; \/\/ \u6307\u5411\u4e0b\u4e00\u4e2a\u7a7a\u4f4d\n\n                            if (rbuf&#91;i] == ',' || rbuf&#91;i] == '*')\n                            {\n                                CntDelimiter++; \/\/ \u5206\u9694\u7b26\u8ba1\u6570\n                                \/\/ std::cout&lt;&lt;\"\u5206\u9694\u7b26\u4e2a\u6570\uff1a\"&lt;&lt;CntDelimiter&lt;&lt;std::endl;\n                                PosDelimiter&#91;CntDelimiter] = CntByte - 1; \/\/ \u8bb0\u4e0b\u5206\u9694\u7b26\u4f4d\u7f6e\n                                \/\/ std::cout&lt;&lt;\"PosDelimiter&#91;\"&lt;&lt;CntDelimiter&lt;&lt;\"]\"&lt;&lt;PosDelimiter&#91;CntDelimiter]&lt;&lt;std::endl;\n                                field_len&#91;CntDelimiter - 1] = PosDelimiter&#91;CntDelimiter] - PosDelimiter&#91;CntDelimiter - 1] - 1;\n                                \/\/ std::cout&lt;&lt;\"\u7b2c\"&lt;&lt;CntDelimiter&lt;&lt;\"\u6bb5\u6570\u636e\u957f\"&lt;&lt;field_len&#91;CntDelimiter]&lt;&lt;std::endl;\n                                if (CntDelimiter == 14) \/\/ 0-14\uff0c\u517115\u4e2a\u5206\u9694\u7b26\uff0c\u5f00\u59cb\u6570\u636e\u89e3\u6790\n                                {\n                                    \/\/ \u8ba1\u7b97\u51fa\u6bcf\u4e2a\u5b57\u6bb5\u7684\u957f\u5ea6\n                                    for (int j = 0; j &lt;= 13; j++) \/\/ 0-13\uff0c22\u4e2a\u5b57\u6bb5\n                                    {\n                                        field_len&#91;j] = PosDelimiter&#91;j + 1] - PosDelimiter&#91;j] - 1;\n                                        \/\/ std::cout&lt;&lt;\"\u7b2c\"&lt;&lt;j&lt;&lt;\"\u6bb5\u6570\u636e\u957f\"&lt;&lt;field_len&#91;j]&lt;&lt;std::endl;\n                                    }\n\n                                    if (field_len&#91;1] > 0)\n                                    {\n                                        memset(temp_field, 0, sizeof(temp_field));\n                                        strncpy(temp_field, &amp;OneFrame&#91;PosDelimiter&#91;1] + 1], field_len&#91;1]);\n                                        int temp = (int)(atof(temp_field) \/ 100);\n                                        gnss_msg->latitude = temp + (atof(temp_field) - temp * 100) \/ 60;\n                                    }\n\n                                    if (field_len&#91;3] > 0)\n                                    {\n                                        memset(temp_field, 0, sizeof(temp_field));\n                                        strncpy(temp_field, &amp;OneFrame&#91;PosDelimiter&#91;3] + 1], field_len&#91;3]);\n                                        int temp = (int)(atof(temp_field) \/ 100);\n                                        gnss_msg->longitude = temp + (atof(temp_field) - temp * 100) \/ 60;\n                                    }\n\n                                    if (field_len&#91;5] > 0)\n                                    {\n                                        memset(temp_field, 0, sizeof(temp_field));\n                                        strncpy(temp_field, &amp;OneFrame&#91;PosDelimiter&#91;5] + 1], field_len&#91;5]);\n                                        gnss_msg->status.status = atof(temp_field);\n                                    }\n\n                                    if (field_len&#91;6] > 0)\n                                    {\n                                        memset(temp_field, 0, sizeof(temp_field));\n                                        strncpy(temp_field, &amp;OneFrame&#91;PosDelimiter&#91;6] + 1], field_len&#91;6]);\n                                        gnss_msg->status.service = atof(temp_field);\n                                    }\n\n                                    if (field_len&#91;7] > 0)\n                                    {\n                                        memset(temp_field, 0, sizeof(temp_field));\n                                        strncpy(temp_field, &amp;OneFrame&#91;PosDelimiter&#91;7] + 1], field_len&#91;7]);\n                                        gnss_msg->position_covariance&#91;0] = pow(atof(temp_field), 2);\n                                        gnss_msg->position_covariance&#91;4] = pow(atof(temp_field), 2);\n                                        gnss_msg->position_covariance&#91;8] = pow(atof(temp_field), 2);\n                                    }\n\n                                    if (field_len&#91;8] > 0)\n                                    {\n                                        memset(temp_field, 0, sizeof(temp_field));\n                                        strncpy(temp_field, &amp;OneFrame&#91;PosDelimiter&#91;8] + 1], field_len&#91;8]);\n                                        gnss_msg->altitude = atof(temp_field);\n                                    }\n\n                                    StateParser = 3;\n                                }\n                            }\n                            break;\n\n                            \/\/ \u6821\u9a8c\u548c\u7b2c\u4e00\u4e2a\u5b57\u7b26\n                        case 3:\n                            OneFrame&#91;CntByte] = rbuf&#91;i];\n                            CntByte++;                                                                                            \/\/ \u6307\u5411\u4e0b\u4e00\u4e2a\u7a7a\u4f4d\n                            if (rbuf&#91;i - 1] == '*' &amp;&amp; ((rbuf&#91;i] >= '0' &amp;&amp; rbuf&#91;i] &lt;= '9') || (rbuf&#91;i] >= 'A' &amp;&amp; rbuf&#91;i] &lt;= 'F'))) \/\/ \u6821\u9a8c\u548c\u5b57\u8282\u5e94\u662f\u4e00\u4e2a\u5341\u516d\u8fdb\u5236\u6570\n                            {\n                                StateParser = 4;\n                            }\n                            else\n                            {\n                                memset(OneFrame, 0, sizeof(OneFrame));\n                                StateParser = 0;\n                            }\n                            break;\n\n                            \/\/ \u6821\u9a8c\u548c\u7b2c\u4e8c\u4e2a\u5b57\u7b26\n                        case 4:\n                            OneFrame&#91;CntByte] = rbuf&#91;i];\n                            CntByte++; \/\/ \u6307\u5411\u4e0b\u4e00\u4e2a\u7a7a\u4f4d\n\n                            if ((rbuf&#91;i] >= '0' &amp;&amp; rbuf&#91;i] &lt;= '9') || (rbuf&#91;i] >= 'A' &amp;&amp; rbuf&#91;i] &lt;= 'F')) \/\/ \u6821\u9a8c\u548c\u5b57\u8282\u5e94\u662f\u4e00\u4e2a\u5341\u516d\u8fdb\u5236\u6570\n                            {\n                                \/\/ \u68c0\u67e5\u6821\u9a8c\n                                cscomputed = GetXorChecksum((char *)(OneFrame + 1), CntByte - 4); \/\/ \u8ba1\u7b97\u5f97\u5230\u7684\u6821\u9a8c\uff0c\u9664\u53bb$*hh&lt;CR>&lt;LF>\u51716\u4e2a\u5b57\u7b26\n                                csreceived = 0;                                                   \/\/ \u63a5\u6536\u5230\u7684\u6821\u9a8c\n                                strtemp&#91;0] = OneFrame&#91;CntByte - 2];\n                                strtemp&#91;1] = OneFrame&#91;CntByte - 1];\n                                strtemp&#91;2] = '\\0';                  \/\/ \u5b57\u7b26\u4e32\u7ed3\u675f\u6807\u5fd7\n                                sscanf(strtemp, \"%x\", &amp;csreceived); \/\/ \u5b57\u7b26\u4e32strtemp\u8f6c\u6362\u4e3a16\u8fdb\u5236\u6570\n\n                                \/\/ \u68c0\u67e5\u6821\u9a8c\u662f\u5426\u6b63\u786e\n                                if (cscomputed != csreceived) \/\/ \u6821\u9a8c\u548c\u4e0d\u5339\u914d\n                                {\n                                    memset(OneFrame, 0, sizeof(OneFrame));\n                                    StateParser = 0;\n                                }\n                                else \/\/ \u6821\u9a8c\u548c\u5339\u914d\n                                {\n                                    StateParser = 5;\n                                }\n                            } \/\/ \u6821\u9a8c\u548c\u5b57\u8282\u662fhex\n                            else\n                            {\n                                memset(OneFrame, 0, sizeof(OneFrame));\n                                StateParser = 0;\n                            }\n\n                            break;\n\n                            \/\/ \u7b49\u5f85\u7ed3\u675f\u6807\u5fd7&lt;CR>=0x0d\n                        case 5:\n                            OneFrame&#91;CntByte] = rbuf&#91;i];\n                            CntByte++; \/\/ \u6307\u5411\u4e0b\u4e00\u4e2a\u7a7a\u4f4d\n                            if (rbuf&#91;i] == '\\r')\n                            {\n                                StateParser = 6;\n                            }\n                            else\n                            {\n                                memset(OneFrame, 0, sizeof(OneFrame));\n                                StateParser = 0;\n                            }\n                            break;\n\n                        \/\/ \u7b49\u5f85\u7ed3\u675f\u6807\u5fd7&lt;LF>=0x0a\n                        case 6:\n                            OneFrame&#91;CntByte] = rbuf&#91;i];\n                            gnss_msg->header.stamp = this->get_clock()->now(); \/\/ ros\u65f6\u523b\n                            gnss_msg->header.frame_id = \"gnss_link\";\n                            gnss_pub_->publish(std::move(gnss_msg)); \/\/ \u53d1\u5e03nav\u6d88\u606f\n                            \/\/ std::cout&lt;&lt;\"\u53d1\u5e03\u6210\u529f\"&lt;&lt;std::endl;\n\n                            memset(OneFrame, 0, sizeof(OneFrame));\n                            StateParser = 0;\n\n                            break;\n\n                        default:\n                            memset(OneFrame, 0, sizeof(OneFrame));\n                            StateParser = 0;\n                            break;\n                        } \/\/ switch(StateParser)\n                    }     \/\/ for(int i=0; i&lt;numgetted; i++)\n                }         \/\/ if(numgetted == numinbuf)\n            }\n            loop_rate.sleep();\n        }\n    }\n\nprivate:\n    \/\/ \u5168\u5c40\u53d8\u91cf\n    serial::Serial ser;      \/\/ \u58f0\u660e\u4e32\u53e3\u5bf9\u8c61\n    int StateParser;         \/\/ \u89e3\u6790\u5904\u7406\u72b6\u6001\u673a\u72b6\u6001\n    int CntByte;             \/\/ \u7528\u4e8e\u8bb0\u5f55OneFrame\u4e2d\u7684\u5b9e\u9645\u6570\u636e\u957f\u5ea6\n    int PosDelimiter&#91;15];    \/\/ \u7528\u4e8e\u8bb0\u5f55\u5206\u9694\u7b26\u4f4d\u7f6e\n    int field_len&#91;14];       \/\/ \u5b57\u7b26\u4e32\u957f\u5ea6\n    int CntDelimiter;        \/\/ \u5206\u9694\u7b26\u8ba1\u6570\n    unsigned char rbuf&#91;500]; \/\/ \u63a5\u6536\u7f13\u51b2\u533a\uff0c\u8981\u8db3\u591f\u5927\uff0c\u9700\u8981\u901a\u8fc7\u6d4b\u8bd5\u5f97\u51fa\n    char OneFrame&#91;250];      \/\/ \u5b58\u653e\u4e00\u5e27\u6570\u636e\uff0c\u957f\u5ea6\u5927\u4e8e115\u5373\u53ef\uff0c\u8fd9\u91cc\u53d6200\n    char str&#91;3];\n    unsigned int tmpint;\n    int cscomputed; \/\/ \u8ba1\u7b97\u5f97\u5230\u7684\u6821\u9a8c\uff0c\u9664\u53bb$*hh&lt;CR>&lt;LF>\u51716\u4e2a\u5b57\u7b26\n    int csreceived; \/\/ \u63a5\u6536\u5230\u7684\u6821\u9a8c\n    char strtemp&#91;3];\n    char temp_field&#91;30];\n    std::string port_name;\n\n    \/*****************************\n    \u529f\u80fd\uff1a\u8ba1\u7b97\u6821\u9a8c\uff0c\u5b57\u7b26\u4e32\u4e2d\u6240\u6709\u5b57\u7b26\u7684\u5f02\u6216\n    \u8fd4\u56de\uff1a\u8fd4\u56de\u4e00\u4e2a\u65e0\u7b26\u53f7\u6574\u6570\n    \u8f93\u5165\u53c2\u6570\uff1a&lt;1>\u5b57\u7b26\u4e32\u6307\u9488\uff0c&lt;2>\u5b57\u7b26\u4e32\u957f\u5ea6\uff08\u6307\u6709\u6548\u957f\u5ea6\uff0c\u4e0d\u5305\u62ec\u5b57\u7b26\u4e32\u7ed3\u675f\u6807\u5fd7\uff09\n    \u8f93\u51fa\u53c2\u6570\uff1a\u6821\u9a8c\u7ed3\u679c\n    ******************************\/\n    unsigned int GetXorChecksum(const char *pch, int len)\n    {\n        unsigned int cs = 0;\n        int i;\n\n        for (i = 0; i &lt; len; i++)\n            cs ^= pch&#91;i];\n\n        return cs;\n    }\n    rclcpp::Publisher&lt;sensor_msgs::msg::NavSatFix>::SharedPtr gnss_pub_;\n};\n\nint main(int argc, char *argv&#91;])\n{\n    rclcpp::init(argc, argv);\n    rclcpp::spin(std::make_shared&lt;GNSSPublisher>(\"gnss_driver_exe\"));\n    rclcpp::shutdown();\n    return 0;\n}\n<\/code><\/pre>\n\n\n\n<p>\u542f\u52a8\uff0c\u518d\u8ba2\u9605GNSS\u6570\u636e\u53ef\u4ee5\u770b\u5230GNSS\u6570\u636e<\/p>\n\n\n\n<pre class=\"wp-block-code\"><code>source install\/setup.bash\nros2 launch m620_driver m620.launch.py\n<\/code><\/pre>\n","protected":false},"excerpt":{"rendered":"<p>\u4e00\u3001\u6fc0\u5149\u96f7\u8fbe\u9a71\u52a8 \u4ee5\u901f\u817e32\u7ebf\u6fc0\u5149\u96f7\u8fbe\u4e3a\u4f8b\uff1a( [&hellip;]<\/p>\n","protected":false},"author":1,"featured_media":0,"comment_status":"open","ping_status":"open","sticky":false,"template":"","format":"standard","meta":{"footnotes":""},"categories":[3,1],"tags":[],"class_list":["post-16749","post","type-post","status-publish","format-standard","hentry","category-technology-frontier","category-home"],"views":1,"_links":{"self":[{"href":"http:\/\/222.128.65.89:18086\/index.php\/wp-json\/wp\/v2\/posts\/16749"}],"collection":[{"href":"http:\/\/222.128.65.89:18086\/index.php\/wp-json\/wp\/v2\/posts"}],"about":[{"href":"http:\/\/222.128.65.89:18086\/index.php\/wp-json\/wp\/v2\/types\/post"}],"author":[{"embeddable":true,"href":"http:\/\/222.128.65.89:18086\/index.php\/wp-json\/wp\/v2\/users\/1"}],"replies":[{"embeddable":true,"href":"http:\/\/222.128.65.89:18086\/index.php\/wp-json\/wp\/v2\/comments?post=16749"}],"version-history":[{"count":1,"href":"http:\/\/222.128.65.89:18086\/index.php\/wp-json\/wp\/v2\/posts\/16749\/revisions"}],"predecessor-version":[{"id":16779,"href":"http:\/\/222.128.65.89:18086\/index.php\/wp-json\/wp\/v2\/posts\/16749\/revisions\/16779"}],"wp:attachment":[{"href":"http:\/\/222.128.65.89:18086\/index.php\/wp-json\/wp\/v2\/media?parent=16749"}],"wp:term":[{"taxonomy":"category","embeddable":true,"href":"http:\/\/222.128.65.89:18086\/index.php\/wp-json\/wp\/v2\/categories?post=16749"},{"taxonomy":"post_tag","embeddable":true,"href":"http:\/\/222.128.65.89:18086\/index.php\/wp-json\/wp\/v2\/tags?post=16749"}],"curies":[{"name":"wp","href":"https:\/\/api.w.org\/{rel}","templated":true}]}}