[{"data":1,"prerenderedAt":5335},["ShallowReactive",2],{"wiki-page-/en-us/wiki/2023-12-30-ros2-tutorial/ch14-6-1-li-cheng-ji-odom":3,"wiki-doc-items-/en-us/wiki/2023-12-30-ros2-tutorial/ch14-6-1-li-cheng-ji-odom":4974,"language-switcher-data-/en-us/wiki/2023-12-30-ros2-tutorial/ch14-6-1-li-cheng-ji-odom":5319,"wiki-i18n-paths-/en-us/wiki/2023-12-30-ros2-tutorial/ch14-6-1-li-cheng-ji-odom":5334},{"id":4,"title":5,"body":6,"canonicalPath":4955,"chapter":4956,"chapterSort":4957,"date":4958,"description":27,"docI18nKey":4959,"docKey":4960,"docRoot":4961,"docTitle":4962,"extension":4963,"i18nKey":4964,"isBlogPost":4965,"isWikiDoc":675,"isWikiIndex":4965,"layout":4966,"legacyPath":4966,"locale":4967,"localeSlug":4968,"meta":4969,"navigation":675,"path":4955,"seo":4970,"sourcePath":4971,"sourceStem":4964,"stem":4972,"wikiDepth":35,"__hash__":4973},"content/_i18n/en-us/wiki/2023-12-30-ros2-tutorial/ch14-6-1-里程计Odom.md","odometry",{"type":7,"value":8,"toc":4950},"minimark",[9,14,18,21,78,81,100,104,115,126,146,175,186,208,213,255,547,550,585,589,596,602,607,612,617,622,625,628,633,636,1324,1327,4938,4943,4946],[10,11,13],"h3",{"id":12},"creating-a-function-package","Creating a function package",[15,16,17],"p",{},"You don't necessarily need encoder (odometry) data. As long as you send all 6 degrees of freedom (DOF) of the robot, which can be calculated from the motor encoders (x, y, z, yaw, pitch, roll — if you're unfamiliar with these, see the MoveIt2 tutorial above), that's sufficient.",[15,19,20],{},"First, create the feature package.",[22,23,28],"pre",{"className":24,"code":25,"language":26,"meta":27,"style":27},"language-bash shiki shiki-themes github-light github-dark","ros2 pkg create odom_plumb --build-type ament_cmake --dependencies rclcpp nav_msgs geometry_msgs tf2_ros --node-name odom_node\n","bash","",[29,30,31],"code",{"__ignoreMap":27},[32,33,36,40,44,47,50,54,57,60,63,66,69,72,75],"span",{"class":34,"line":35},"line",1,[32,37,39],{"class":38},"sScJk","ros2",[32,41,43],{"class":42},"sZZnC"," pkg",[32,45,46],{"class":42}," create",[32,48,49],{"class":42}," odom_plumb",[32,51,53],{"class":52},"sj4cs"," --build-type",[32,55,56],{"class":42}," ament_cmake",[32,58,59],{"class":52}," --dependencies",[32,61,62],{"class":42}," rclcpp",[32,64,65],{"class":42}," nav_msgs",[32,67,68],{"class":42}," geometry_msgs",[32,70,71],{"class":42}," tf2_ros",[32,73,74],{"class":52}," --node-name",[32,76,77],{"class":42}," odom_node\n",[15,79,80],{},"Let's compile it first.",[22,82,84],{"className":24,"code":83,"language":26,"meta":27,"style":27},"colcon build --cmake-args -DCMAKE_EXPORT_COMPILE_COMMANDS=ON\n",[29,85,86],{"__ignoreMap":27},[32,87,88,91,94,97],{"class":34,"line":35},[32,89,90],{"class":38},"colcon",[32,92,93],{"class":42}," build",[32,95,96],{"class":52}," --cmake-args",[32,98,99],{"class":52}," -DCMAKE_EXPORT_COMPILE_COMMANDS=ON\n",[10,101,103],{"id":102},"message-interface","Message interface",[15,105,106,107,110,111,114],{},"After the chassis driver starts, it will continuously broadcast a critical piece of data—the odometry. Odometry describes the current robot's pose and velocity relative to its starting point, and plays an important role in robot localization and local path planning for navigation. In ROS2, the odometry message is encapsulated as the ",[29,108,109],{},"nav_msgs/msg/Odometry"," interface. You can view the specific data structure of this interface using the command ",[29,112,113],{},"ros2 interface show nav_msgs/msg/Odometry",", with the results shown below:",[15,116,117,118,121,122,125],{},"It describes the estimated information of the robot's pose and velocity. The pose information is referenced to the ",[29,119,120],{},"frame_id"," in the header, while the velocity information is referenced to the ",[29,123,124],{},"child_frame_id",".",[15,127,128,129,133,134,137,138,141,142,145],{},"In ",[130,131,132],"strong",{},"ROS2",", the ",[130,135,136],{},"odom coordinate frame"," (usually the ",[29,139,140],{},"odom"," frame) follows the ",[130,143,144],{},"right-hand rule",", and is defined as follows:",[147,148,149,159,167],"ul",{},[150,151,152,155,156,125],"li",{},[130,153,154],{},"X-axis",": Forward (direction of travel) is ",[130,157,158],{},"positive",[150,160,161,164,165,125],{},[130,162,163],{},"Y-axis",": Left (leftward direction) is ",[130,166,158],{},[150,168,169,172,173,125],{},[130,170,171],{},"Z-axis",": Upward (perpendicular to the ground) is ",[130,174,158],{},[15,176,177,178,181,182,185],{},"In the ",[130,179,180],{},"ROS2 odom coordinate system"," (right-handed coordinate system), the ",[130,183,184],{},"definition of the yaw angle"," is as follows:",[147,187,188,198,203],{},[150,189,190,193,194,197],{},[130,191,192],{},"yaw = 0"," : The robot faces the ",[130,195,196],{},"positive X-axis direction"," (forward).",[150,199,200],{},[130,201,202],{},"Counterclockwise (CCW) rotation is positive.",[150,204,205],{},[130,206,207],{},"Clockwise (CW) rotation is negative.",[15,209,210],{},[130,211,212],{},"yaw angle range",[147,214,215,232,246],{},[150,216,217,220,221],{},[29,218,219],{},"yaw ∈ (-π, π]"," ",[130,222,223,224,227,228,231],{},"(i.e., from ",[29,225,226],{},"-180°"," to ",[29,229,230],{},"180°",")",[150,233,234,235,238,239,242,243,125],{},"When ",[29,236,237],{},"yaw = π"," (180°), if it continues to increase, it should jump to ",[29,240,241],{},"-π"," (-180°), rather than ",[29,244,245],{},"π",[150,247,234,248,251,252,254],{},[29,249,250],{},"yaw = -π"," (-180°), if it continues to decrease, it should jump to ",[29,253,245],{}," (180°).",[22,256,258],{"className":24,"code":257,"language":26,"meta":27,"style":27},"std_msgs/Header header # 标头\n    builtin_interfaces/Time stamp # 时间戳\n        int32 sec\n        uint32 nanosec\n    string frame_id # 位姿信息所参考的坐标系\n位姿信息所指向的坐标系，也是速度信息所参考的坐标系\nstring child_frame_id\n相对于frame_id的预估位姿信息\ngeometry_msgs/PoseWithCovariance pose\n    Pose pose\n        Point position # 坐标\n            float64 x\n            float64 y\n            float64 z\n        Quaternion orientation # 四元数\n            float64 x 0\n            float64 y 0\n            float64 z 0\n            float64 w 1\n    float64[36] covariance\n相对于child_frame_id的预估线速度与角速度\ngeometry_msgs/TwistWithCovariance twist\n    Twist twist\n        Vector3  linear # 线速度\n            float64 x\n            float64 y\n            float64 z\n        Vector3  angular # 角速度\n            float64 x\n            float64 y\n            float64 z\n    float64[36] covariance\n",[29,259,260,272,284,293,302,314,320,329,335,344,352,364,373,381,389,401,412,422,432,443,452,458,467,475,487,494,501,508,519,526,533,540],{"__ignoreMap":27},[32,261,262,265,268],{"class":34,"line":35},[32,263,264],{"class":38},"std_msgs/Header",[32,266,267],{"class":42}," header",[32,269,271],{"class":270},"sJ8bj"," # 标头\n",[32,273,275,278,281],{"class":34,"line":274},2,[32,276,277],{"class":38},"    builtin_interfaces/Time",[32,279,280],{"class":42}," stamp",[32,282,283],{"class":270}," # 时间戳\n",[32,285,287,290],{"class":34,"line":286},3,[32,288,289],{"class":38},"        int32",[32,291,292],{"class":42}," sec\n",[32,294,296,299],{"class":34,"line":295},4,[32,297,298],{"class":38},"        uint32",[32,300,301],{"class":42}," nanosec\n",[32,303,305,308,311],{"class":34,"line":304},5,[32,306,307],{"class":38},"    string",[32,309,310],{"class":42}," frame_id",[32,312,313],{"class":270}," # 位姿信息所参考的坐标系\n",[32,315,317],{"class":34,"line":316},6,[32,318,319],{"class":38},"位姿信息所指向的坐标系，也是速度信息所参考的坐标系\n",[32,321,323,326],{"class":34,"line":322},7,[32,324,325],{"class":38},"string",[32,327,328],{"class":42}," child_frame_id\n",[32,330,332],{"class":34,"line":331},8,[32,333,334],{"class":38},"相对于frame_id的预估位姿信息\n",[32,336,338,341],{"class":34,"line":337},9,[32,339,340],{"class":38},"geometry_msgs/PoseWithCovariance",[32,342,343],{"class":42}," pose\n",[32,345,347,350],{"class":34,"line":346},10,[32,348,349],{"class":38},"    Pose",[32,351,343],{"class":42},[32,353,355,358,361],{"class":34,"line":354},11,[32,356,357],{"class":38},"        Point",[32,359,360],{"class":42}," position",[32,362,363],{"class":270}," # 坐标\n",[32,365,367,370],{"class":34,"line":366},12,[32,368,369],{"class":38},"            float64",[32,371,372],{"class":42}," x\n",[32,374,376,378],{"class":34,"line":375},13,[32,377,369],{"class":38},[32,379,380],{"class":42}," y\n",[32,382,384,386],{"class":34,"line":383},14,[32,385,369],{"class":38},[32,387,388],{"class":42}," z\n",[32,390,392,395,398],{"class":34,"line":391},15,[32,393,394],{"class":38},"        Quaternion",[32,396,397],{"class":42}," orientation",[32,399,400],{"class":270}," # 四元数\n",[32,402,404,406,409],{"class":34,"line":403},16,[32,405,369],{"class":38},[32,407,408],{"class":42}," x",[32,410,411],{"class":52}," 0\n",[32,413,415,417,420],{"class":34,"line":414},17,[32,416,369],{"class":38},[32,418,419],{"class":42}," y",[32,421,411],{"class":52},[32,423,425,427,430],{"class":34,"line":424},18,[32,426,369],{"class":38},[32,428,429],{"class":42}," z",[32,431,411],{"class":52},[32,433,435,437,440],{"class":34,"line":434},19,[32,436,369],{"class":38},[32,438,439],{"class":42}," w",[32,441,442],{"class":52}," 1\n",[32,444,446,449],{"class":34,"line":445},20,[32,447,448],{"class":38},"    float64[36]",[32,450,451],{"class":42}," covariance\n",[32,453,455],{"class":34,"line":454},21,[32,456,457],{"class":38},"相对于child_frame_id的预估线速度与角速度\n",[32,459,461,464],{"class":34,"line":460},22,[32,462,463],{"class":38},"geometry_msgs/TwistWithCovariance",[32,465,466],{"class":42}," twist\n",[32,468,470,473],{"class":34,"line":469},23,[32,471,472],{"class":38},"    Twist",[32,474,466],{"class":42},[32,476,478,481,484],{"class":34,"line":477},24,[32,479,480],{"class":38},"        Vector3",[32,482,483],{"class":42},"  linear",[32,485,486],{"class":270}," # 线速度\n",[32,488,490,492],{"class":34,"line":489},25,[32,491,369],{"class":38},[32,493,372],{"class":42},[32,495,497,499],{"class":34,"line":496},26,[32,498,369],{"class":38},[32,500,380],{"class":42},[32,502,504,506],{"class":34,"line":503},27,[32,505,369],{"class":38},[32,507,388],{"class":42},[32,509,511,513,516],{"class":34,"line":510},28,[32,512,480],{"class":38},[32,514,515],{"class":42},"  angular",[32,517,518],{"class":270}," # 角速度\n",[32,520,522,524],{"class":34,"line":521},29,[32,523,369],{"class":38},[32,525,372],{"class":42},[32,527,529,531],{"class":34,"line":528},30,[32,530,369],{"class":38},[32,532,380],{"class":42},[32,534,536,538],{"class":34,"line":535},31,[32,537,369],{"class":38},[32,539,388],{"class":42},[32,541,543,545],{"class":34,"line":542},32,[32,544,448],{"class":38},[32,546,451],{"class":42},[15,548,549],{},"It is important to note that,",[551,552,553,559,568,582],"ol",{},[150,554,555,556,558],{},"Coordinates and poses are relative to ",[130,557,120],{},", which is typically odom itself.",[150,560,561,562,564,565,567],{},"Linear velocity and angular velocity are relative to ",[130,563,124],{},". Typically, this is base_link ",[130,566,125],{}," (Note: I made this mistake myself at first.)",[150,569,570,571,574,575,578,579,581],{},"Although Twist is relative to child_frame_id ",[130,572,573],{},"("," base_link), it expresses an ",[130,576,577],{},"instantaneous value",", just like a car — the speedometer in a car also shows an ",[130,580,577],{}," relative to the vehicle body.",[150,583,584],{},"Odometry accumulates errors during calculation, so it describes the robot's estimated pose, which is not absolutely accurate.",[10,586,588],{"id":587},"node-main-logic","Node main logic",[15,590,591,592,595],{},"So we need to initialize ",[29,593,594],{},"nav_msgs::msg::Odometry"," and publish it.",[15,597,598],{},[599,600],"img",{"alt":27,"src":601},"https://cdn.tungchiahui.cn/tungwebsite/assets/images/2023/12/30/image1794.webp",[15,603,604],{},[599,605],{"alt":27,"src":606},"https://cdn.tungchiahui.cn/tungwebsite/assets/images/2023/12/30/image1795.webp",[15,608,609],{},[599,610],{"alt":27,"src":611},"https://cdn.tungchiahui.cn/tungwebsite/assets/images/2023/12/30/image1796.webp",[15,613,614],{},[599,615],{"alt":27,"src":616},"https://cdn.tungchiahui.cn/tungwebsite/assets/images/2023/12/30/image1797.webp",[15,618,619,620,125],{},"So you need to obtain or compute this data from the MCU microcontroller and send it out as a message of type ",[29,621,594],{},[15,623,624],{},"Note: If you don't have a proper odometer, the odometry data can be calculated from the chassis motor encoder data.",[15,626,627],{},"In addition to the code for publishing topics, don't forget to publish the dynamic coordinate transform between odom and base_link.",[15,629,630],{},[599,631],{"alt":27,"src":632},"https://cdn.tungchiahui.cn/tungwebsite/assets/images/2023/12/30/image1798.webp",[15,634,635],{},"Below is the odometry data calculated from forward kinematics:",[22,637,641],{"className":638,"code":639,"language":640,"meta":27,"style":27},"language-cpp shiki shiki-themes github-light github-dark","#include \"odom_mg513.h\"\n#include \"mg513_gmr500ppr.h\"\n//#include \u003Ccmath>\n#include \"arm_math.h\"\n\nODOM_Motor odom_motor_;\n\n// 坐标系满足右手坐标系\n\n//（单位：米）\n//轮距\nextern fp32 Wheel_Spacing;\n//轴距\nextern fp32 Alex_Spacing;\n//轮子半径\nextern fp32 Wheel_Radius;\n\nvoid ODOM_Motor::Analysis(fp32 dt)\n{\n  this->dt = dt;\n  for(int16_t i = 0;i \u003C 4;i++)\n    {\n        // 将 RPM 转换为 m/s\n      encoder_wheel_velocities_[i] = mg513_gmr500ppr_motor[i].encoder.motor_data.motor_speed  * 2.0f * 3.14159265358979f * Wheel_Radius / 60.0f;\n    }\n    // 计算机器人前进的线速度和角速度，公式不需要轮半径\n    //线速度，四个轮子在机器人的运动学模型中贡献相同，所以要除以4\n    this->vx = ( encoder_wheel_velocities_[0] + encoder_wheel_velocities_[1] - encoder_wheel_velocities_[2] - encoder_wheel_velocities_[3]) / 4.0f;\n    this->vy = ( encoder_wheel_velocities_[0] - encoder_wheel_velocities_[1] - encoder_wheel_velocities_[2] + encoder_wheel_velocities_[3]) / 4.0f;\n\n    //线速度，轮距（wheel_spacing） 决定了左右轮对旋转的贡献程度，轴距（alex_spacing） 决定了前后轮对旋转的贡献程度，\n    //所以要除以底盘尺寸，alex_spacing + wheel_spacing 是底盘尺寸。\n    this->vw = (-encoder_wheel_velocities_[0] - encoder_wheel_velocities_[1] - encoder_wheel_velocities_[2] - encoder_wheel_velocities_[3]) / (4.0f * (Wheel_Spacing + Alex_Spacing));\n\n    // 更新机器人的位置（假设机器人沿着y轴移动）\n//     this->x_position += this->vx * std::__math::cos(this->yaw) * this->dt;  \n//     this->y_position += this->vy * std::__math::sin(this->yaw) * this->dt;\n    this->x_position += this->vx * arm_cos_f32(this->yaw) * this->dt;  \n    this->y_position += this->vy * arm_sin_f32(this->yaw) * this->dt;\n    this->yaw += this->vw * this->dt;\n\n    // 保证 yaw 始终在 -PI 到 PI 之间\n    if(this->yaw > 3.14159265358979f) \n    {\n      this->yaw -= 2.0 * 3.14159265358979f;\n    }\n    if(this->yaw \u003C -3.14159265358979f) \n    {\n      this->yaw += 2.0 * 3.14159265358979f;\n    }\n\n//    this->yaw_deg = this->yaw *  180.0f / 3.14159265358979f;\n\n}\n\n","cpp",[29,642,643,652,659,664,671,677,683,687,692,696,701,706,714,719,726,731,738,742,768,773,787,822,827,832,875,880,885,890,949,996,1000,1005,1010,1075,1080,1086,1092,1098,1134,1166,1186,1191,1197,1219,1224,1245,1250,1273,1278,1297,1302,1307,1313,1318],{"__ignoreMap":27},[32,644,645,649],{"class":34,"line":35},[32,646,648],{"class":647},"szBVR","#include",[32,650,651],{"class":42}," \"odom_mg513.h\"\n",[32,653,654,656],{"class":34,"line":274},[32,655,648],{"class":647},[32,657,658],{"class":42}," \"mg513_gmr500ppr.h\"\n",[32,660,661],{"class":34,"line":286},[32,662,663],{"class":270},"//#include \u003Ccmath>\n",[32,665,666,668],{"class":34,"line":295},[32,667,648],{"class":647},[32,669,670],{"class":42}," \"arm_math.h\"\n",[32,672,673],{"class":34,"line":304},[32,674,676],{"emptyLinePlaceholder":675},true,"\n",[32,678,679],{"class":34,"line":316},[32,680,682],{"class":681},"sVt8B","ODOM_Motor odom_motor_;\n",[32,684,685],{"class":34,"line":322},[32,686,676],{"emptyLinePlaceholder":675},[32,688,689],{"class":34,"line":331},[32,690,691],{"class":270},"// 坐标系满足右手坐标系\n",[32,693,694],{"class":34,"line":337},[32,695,676],{"emptyLinePlaceholder":675},[32,697,698],{"class":34,"line":346},[32,699,700],{"class":270},"//（单位：米）\n",[32,702,703],{"class":34,"line":354},[32,704,705],{"class":270},"//轮距\n",[32,707,708,711],{"class":34,"line":366},[32,709,710],{"class":647},"extern",[32,712,713],{"class":681}," fp32 Wheel_Spacing;\n",[32,715,716],{"class":34,"line":375},[32,717,718],{"class":270},"//轴距\n",[32,720,721,723],{"class":34,"line":383},[32,722,710],{"class":647},[32,724,725],{"class":681}," fp32 Alex_Spacing;\n",[32,727,728],{"class":34,"line":391},[32,729,730],{"class":270},"//轮子半径\n",[32,732,733,735],{"class":34,"line":403},[32,734,710],{"class":647},[32,736,737],{"class":681}," fp32 Wheel_Radius;\n",[32,739,740],{"class":34,"line":414},[32,741,676],{"emptyLinePlaceholder":675},[32,743,744,747,750,753,756,758,761,765],{"class":34,"line":424},[32,745,746],{"class":647},"void",[32,748,749],{"class":38}," ODOM_Motor",[32,751,752],{"class":681},"::",[32,754,755],{"class":38},"Analysis",[32,757,573],{"class":681},[32,759,760],{"class":38},"fp32",[32,762,764],{"class":763},"s4XuR"," dt",[32,766,767],{"class":681},")\n",[32,769,770],{"class":34,"line":434},[32,771,772],{"class":681},"{\n",[32,774,775,778,781,784],{"class":34,"line":445},[32,776,777],{"class":52},"  this",[32,779,780],{"class":681},"->dt ",[32,782,783],{"class":647},"=",[32,785,786],{"class":681}," dt;\n",[32,788,789,792,794,797,800,802,805,808,811,814,817,820],{"class":34,"line":454},[32,790,791],{"class":647},"  for",[32,793,573],{"class":681},[32,795,796],{"class":647},"int16_t",[32,798,799],{"class":681}," i ",[32,801,783],{"class":647},[32,803,804],{"class":52}," 0",[32,806,807],{"class":681},";i ",[32,809,810],{"class":647},"\u003C",[32,812,813],{"class":52}," 4",[32,815,816],{"class":681},";i",[32,818,819],{"class":647},"++",[32,821,767],{"class":681},[32,823,824],{"class":34,"line":460},[32,825,826],{"class":681},"    {\n",[32,828,829],{"class":34,"line":469},[32,830,831],{"class":270},"        // 将 RPM 转换为 m/s\n",[32,833,834,837,839,842,845,848,851,854,857,859,861,864,867,870,872],{"class":34,"line":477},[32,835,836],{"class":681},"      encoder_wheel_velocities_[i] ",[32,838,783],{"class":647},[32,840,841],{"class":681}," mg513_gmr500ppr_motor[i].encoder.motor_data.motor_speed  ",[32,843,844],{"class":647},"*",[32,846,847],{"class":52}," 2.0",[32,849,850],{"class":647},"f",[32,852,853],{"class":647}," *",[32,855,856],{"class":52}," 3.14159265358979",[32,858,850],{"class":647},[32,860,853],{"class":647},[32,862,863],{"class":681}," Wheel_Radius ",[32,865,866],{"class":647},"/",[32,868,869],{"class":52}," 60.0",[32,871,850],{"class":647},[32,873,874],{"class":681},";\n",[32,876,877],{"class":34,"line":489},[32,878,879],{"class":681},"    }\n",[32,881,882],{"class":34,"line":496},[32,883,884],{"class":270},"    // 计算机器人前进的线速度和角速度，公式不需要轮半径\n",[32,886,887],{"class":34,"line":503},[32,888,889],{"class":270},"    //线速度，四个轮子在机器人的运动学模型中贡献相同，所以要除以4\n",[32,891,892,895,898,900,903,906,909,912,915,918,920,923,925,928,930,932,934,937,940,942,945,947],{"class":34,"line":510},[32,893,894],{"class":52},"    this",[32,896,897],{"class":681},"->vx ",[32,899,783],{"class":647},[32,901,902],{"class":681}," ( encoder_wheel_velocities_[",[32,904,905],{"class":52},"0",[32,907,908],{"class":681},"] ",[32,910,911],{"class":647},"+",[32,913,914],{"class":681}," encoder_wheel_velocities_[",[32,916,917],{"class":52},"1",[32,919,908],{"class":681},[32,921,922],{"class":647},"-",[32,924,914],{"class":681},[32,926,927],{"class":52},"2",[32,929,908],{"class":681},[32,931,922],{"class":647},[32,933,914],{"class":681},[32,935,936],{"class":52},"3",[32,938,939],{"class":681},"]) ",[32,941,866],{"class":647},[32,943,944],{"class":52}," 4.0",[32,946,850],{"class":647},[32,948,874],{"class":681},[32,950,951,953,956,958,960,962,964,966,968,970,972,974,976,978,980,982,984,986,988,990,992,994],{"class":34,"line":521},[32,952,894],{"class":52},[32,954,955],{"class":681},"->vy ",[32,957,783],{"class":647},[32,959,902],{"class":681},[32,961,905],{"class":52},[32,963,908],{"class":681},[32,965,922],{"class":647},[32,967,914],{"class":681},[32,969,917],{"class":52},[32,971,908],{"class":681},[32,973,922],{"class":647},[32,975,914],{"class":681},[32,977,927],{"class":52},[32,979,908],{"class":681},[32,981,911],{"class":647},[32,983,914],{"class":681},[32,985,936],{"class":52},[32,987,939],{"class":681},[32,989,866],{"class":647},[32,991,944],{"class":52},[32,993,850],{"class":647},[32,995,874],{"class":681},[32,997,998],{"class":34,"line":528},[32,999,676],{"emptyLinePlaceholder":675},[32,1001,1002],{"class":34,"line":535},[32,1003,1004],{"class":270},"    //线速度，轮距（wheel_spacing） 决定了左右轮对旋转的贡献程度，轴距（alex_spacing） 决定了前后轮对旋转的贡献程度，\n",[32,1006,1007],{"class":34,"line":542},[32,1008,1009],{"class":270},"    //所以要除以底盘尺寸，alex_spacing + wheel_spacing 是底盘尺寸。\n",[32,1011,1013,1015,1018,1020,1023,1025,1028,1030,1032,1034,1036,1038,1040,1042,1044,1046,1048,1050,1052,1054,1056,1058,1060,1063,1065,1067,1070,1072],{"class":34,"line":1012},33,[32,1014,894],{"class":52},[32,1016,1017],{"class":681},"->vw ",[32,1019,783],{"class":647},[32,1021,1022],{"class":681}," (",[32,1024,922],{"class":647},[32,1026,1027],{"class":681},"encoder_wheel_velocities_[",[32,1029,905],{"class":52},[32,1031,908],{"class":681},[32,1033,922],{"class":647},[32,1035,914],{"class":681},[32,1037,917],{"class":52},[32,1039,908],{"class":681},[32,1041,922],{"class":647},[32,1043,914],{"class":681},[32,1045,927],{"class":52},[32,1047,908],{"class":681},[32,1049,922],{"class":647},[32,1051,914],{"class":681},[32,1053,936],{"class":52},[32,1055,939],{"class":681},[32,1057,866],{"class":647},[32,1059,1022],{"class":681},[32,1061,1062],{"class":52},"4.0",[32,1064,850],{"class":647},[32,1066,853],{"class":647},[32,1068,1069],{"class":681}," (Wheel_Spacing ",[32,1071,911],{"class":647},[32,1073,1074],{"class":681}," Alex_Spacing));\n",[32,1076,1078],{"class":34,"line":1077},34,[32,1079,676],{"emptyLinePlaceholder":675},[32,1081,1083],{"class":34,"line":1082},35,[32,1084,1085],{"class":270},"    // 更新机器人的位置（假设机器人沿着y轴移动）\n",[32,1087,1089],{"class":34,"line":1088},36,[32,1090,1091],{"class":270},"//     this->x_position += this->vx * std::__math::cos(this->yaw) * this->dt;  \n",[32,1093,1095],{"class":34,"line":1094},37,[32,1096,1097],{"class":270},"//     this->y_position += this->vy * std::__math::sin(this->yaw) * this->dt;\n",[32,1099,1101,1103,1106,1109,1112,1114,1116,1119,1121,1124,1127,1129,1131],{"class":34,"line":1100},38,[32,1102,894],{"class":52},[32,1104,1105],{"class":681},"->x_position ",[32,1107,1108],{"class":647},"+=",[32,1110,1111],{"class":52}," this",[32,1113,897],{"class":681},[32,1115,844],{"class":647},[32,1117,1118],{"class":38}," arm_cos_f32",[32,1120,573],{"class":681},[32,1122,1123],{"class":52},"this",[32,1125,1126],{"class":681},"->yaw) ",[32,1128,844],{"class":647},[32,1130,1111],{"class":52},[32,1132,1133],{"class":681},"->dt;  \n",[32,1135,1137,1139,1142,1144,1146,1148,1150,1153,1155,1157,1159,1161,1163],{"class":34,"line":1136},39,[32,1138,894],{"class":52},[32,1140,1141],{"class":681},"->y_position ",[32,1143,1108],{"class":647},[32,1145,1111],{"class":52},[32,1147,955],{"class":681},[32,1149,844],{"class":647},[32,1151,1152],{"class":38}," arm_sin_f32",[32,1154,573],{"class":681},[32,1156,1123],{"class":52},[32,1158,1126],{"class":681},[32,1160,844],{"class":647},[32,1162,1111],{"class":52},[32,1164,1165],{"class":681},"->dt;\n",[32,1167,1169,1171,1174,1176,1178,1180,1182,1184],{"class":34,"line":1168},40,[32,1170,894],{"class":52},[32,1172,1173],{"class":681},"->yaw ",[32,1175,1108],{"class":647},[32,1177,1111],{"class":52},[32,1179,1017],{"class":681},[32,1181,844],{"class":647},[32,1183,1111],{"class":52},[32,1185,1165],{"class":681},[32,1187,1189],{"class":34,"line":1188},41,[32,1190,676],{"emptyLinePlaceholder":675},[32,1192,1194],{"class":34,"line":1193},42,[32,1195,1196],{"class":270},"    // 保证 yaw 始终在 -PI 到 PI 之间\n",[32,1198,1200,1203,1205,1207,1209,1212,1214,1216],{"class":34,"line":1199},43,[32,1201,1202],{"class":647},"    if",[32,1204,573],{"class":681},[32,1206,1123],{"class":52},[32,1208,1173],{"class":681},[32,1210,1211],{"class":647},">",[32,1213,856],{"class":52},[32,1215,850],{"class":647},[32,1217,1218],{"class":681},") \n",[32,1220,1222],{"class":34,"line":1221},44,[32,1223,826],{"class":681},[32,1225,1227,1230,1232,1235,1237,1239,1241,1243],{"class":34,"line":1226},45,[32,1228,1229],{"class":52},"      this",[32,1231,1173],{"class":681},[32,1233,1234],{"class":647},"-=",[32,1236,847],{"class":52},[32,1238,853],{"class":647},[32,1240,856],{"class":52},[32,1242,850],{"class":647},[32,1244,874],{"class":681},[32,1246,1248],{"class":34,"line":1247},46,[32,1249,879],{"class":681},[32,1251,1253,1255,1257,1259,1261,1263,1266,1269,1271],{"class":34,"line":1252},47,[32,1254,1202],{"class":647},[32,1256,573],{"class":681},[32,1258,1123],{"class":52},[32,1260,1173],{"class":681},[32,1262,810],{"class":647},[32,1264,1265],{"class":647}," -",[32,1267,1268],{"class":52},"3.14159265358979",[32,1270,850],{"class":647},[32,1272,1218],{"class":681},[32,1274,1276],{"class":34,"line":1275},48,[32,1277,826],{"class":681},[32,1279,1281,1283,1285,1287,1289,1291,1293,1295],{"class":34,"line":1280},49,[32,1282,1229],{"class":52},[32,1284,1173],{"class":681},[32,1286,1108],{"class":647},[32,1288,847],{"class":52},[32,1290,853],{"class":647},[32,1292,856],{"class":52},[32,1294,850],{"class":647},[32,1296,874],{"class":681},[32,1298,1300],{"class":34,"line":1299},50,[32,1301,879],{"class":681},[32,1303,1305],{"class":34,"line":1304},51,[32,1306,676],{"emptyLinePlaceholder":675},[32,1308,1310],{"class":34,"line":1309},52,[32,1311,1312],{"class":270},"//    this->yaw_deg = this->yaw *  180.0f / 3.14159265358979f;\n",[32,1314,1316],{"class":34,"line":1315},53,[32,1317,676],{"emptyLinePlaceholder":675},[32,1319,1321],{"class":34,"line":1320},54,[32,1322,1323],{"class":681},"}\n",[15,1325,1326],{},"The total ROS2 code is as follows:",[22,1328,1330],{"className":638,"code":1329,"language":640,"meta":27,"style":27},"#include \"rclcpp/rclcpp.hpp\"\n#include \"serial_driver/serial_driver.hpp\"\n#include \u003Cchrono>\n#include \u003Cfunctional>\n#include \u003Crclcpp/logging.hpp>\n#include \u003Cstring>\n#include \"sensor04_odom_kc/serial_pack.h\"\n#include \"geometry_msgs/msg/twist.hpp\"\n#include \"nav_msgs/msg/odometry.hpp\"\n#include \u003Ctf2/LinearMath/Quaternion.h>\n#include \u003Ctf2_ros/transform_broadcaster.h>\n\nextern std::vector\u003Cuint8_t> tx_data_buffer;\n\nclass Odom_KC_Node : public rclcpp::Node\n{\npublic:\n  Odom_KC_Node() : Node(\"Odom_KC_Node\")\n  {\n    // 声明参数（带默认值）\n    // 串口设备默认名（根据实际设备调整）\n    this->declare_parameter(\"device_name\", \"/dev/ttyACM1\");\n    //串口默认波特率\n    this->declare_parameter(\"baud_rate\", 115200);\n\n    // 获取参数值\n    const std::string device_name = this->get_parameter(\"device_name\").as_string();\n    const uint32_t baud_rate = this->get_parameter(\"baud_rate\").as_int();\n\n    RCLCPP_INFO(this->get_logger(), \"Serial port Node Open!\");\n    // 创建串口配置对象\n    // 波特率默认115200；不开启流控制；无奇偶效验；停止位1。\n    drivers::serial_driver::SerialPortConfig config(\n        baud_rate,\n        drivers::serial_driver::FlowControl::NONE,\n        drivers::serial_driver::Parity::NONE,\n        drivers::serial_driver::StopBits::ONE);\n\n    // 初始化串口\n    try\n    {\n      io_context_ = std::make_shared\u003Cdrivers::common::IoContext>(1);\n      // 初始化 serial_driver_\n      serial_driver_ = std::make_shared\u003Cdrivers::serial_driver::SerialDriver>(*io_context_);\n      serial_driver_->init_port(device_name, config);\n      serial_driver_->port()->open();\n\n      RCLCPP_INFO(this->get_logger(), \"Serial port initialized successfully\");\n      RCLCPP_INFO(this->get_logger(), \"Using device: %s\", serial_driver_->port().get()->device_name().c_str());\n      RCLCPP_INFO(this->get_logger(), \"Baud_rate: %d\", config.get_baud_rate());\n    }\n    catch (const std::exception &ex)\n    {\n      RCLCPP_ERROR(this->get_logger(), \"Failed to initialize serial port: %s\", ex.what());\n      return;\n    }\n\n    //串口发布方\n    odom_pub_ = this->create_publisher\u003Cnav_msgs::msg::Odometry>(\"odom\",10);\n\n    //初始化tf广播器\n    tf_broadcaster_ = std::make_unique\u003Ctf2_ros::TransformBroadcaster>(*this);\n\n    //初始化cmd_vel消息订阅\n    cmd_vel_sub_ = this->create_subscription\u003Cgeometry_msgs::msg::Twist>(\"cmd_vel\",10,std::bind(&Odom_KC_Node::cmd_vel_sub_callback,this,std::placeholders::_1));\n\n    async_receive_message();   //进入异步接收\n  }\n\nprivate:\n  std::shared_ptr\u003Cdrivers::serial_driver::SerialDriver> serial_driver_;\n  std::shared_ptr\u003Cdrivers::common::IoContext> io_context_;\n\n  rclcpp::Publisher\u003Cnav_msgs::msg::Odometry>::SharedPtr odom_pub_;\n  std::unique_ptr\u003Ctf2_ros::TransformBroadcaster> tf_broadcaster_;\n\n  rclcpp::Subscription\u003Cgeometry_msgs::msg::Twist>::SharedPtr cmd_vel_sub_;\n\n  // // 四个轮子的速度(对应单片机的顺序)（单位：rpm）\n  // fp32 received_encoder_wheel_velocities_[4] = {0.0, 0.0, 0.0, 0.0};\n  // // 四个轮子的速度(对应单片机的顺序)（单位：m/s）\n  // fp32 encoder_wheel_velocities_[4] = {0.0, 0.0, 0.0, 0.0};  \n\n  // // 四个轮子的速度(对应单片机的顺序)（单位：2000pc）\n  // fp32 received_encoder_wheel_angle_[4] = {0.0, 0.0, 0.0, 0.0};\n\n  //麦克纳姆轮底盘参数\n  const fp32 wheel_spacing = 0.093; // 轮间距（单位：米）\n  const fp32 alex_spacing = 0.085; // 轮距（单位：米）\n  const fp32 wheel_radius_ = 0.0375; // 轮子半径（单位：米）\n\n  fp32 vy=0.0,vx=0.0,vw=0.0;\n  fp32 yaw_ = 0.0; // 机器人航向角（单位：弧度）\n  fp32 dt_;\n\n  fp32 x_position_ = 0.0; // x位置\n  fp32 y_position_ = 0.0; // y位置\n\n  void async_receive_message()  //创建一个函数更方便重新调用\n  {\n    auto port = serial_driver_->port();\n\n    // 设置接收回调函数\n    port->async_receive([this](const std::vector\u003Cuint8_t> &data,const size_t &size) \n    {\n      //创建odom消息类型\n      auto odom_msg = nav_msgs::msg::Odometry();\n\n        if (size > 0)\n        {\n          for(int16_t i = 0;i \u003C (int16_t)size;++i)\n          {\n            uint8_t data_buffer = data[i];\n            // 处理接收到的数据\n            serial_pack_.rx.Data_Analysis(\n              &data_buffer,\n              0x01,\n            0,\n            0,\n            0,\n            0,\n            8);\n          }\n          //由于在ROS2中，node是局部变量，所以发布方只能在node类里，故Data_Apply不写任何东西，直接在接收下面的回调函数里实现功能。\n          if(serial_pack_.rx.data.cmd == 0x01)\n          {\n            RCLCPP_DEBUG(this->get_logger(), \"\\n\");\n            RCLCPP_DEBUG(this->get_logger(), \"以下是电机编码器的odom数据：\");\n\n            // // 存储电机速度数据\n            // received_encoder_wheel_velocities_[0] = serial_pack_.rx.data.fp32_buffer[0];  // 电机 0 速度\n            // received_encoder_wheel_velocities_[1] = serial_pack_.rx.data.fp32_buffer[1];  // 电机 1 速度\n            // received_encoder_wheel_velocities_[2] = serial_pack_.rx.data.fp32_buffer[2];  // 电机 2 速度\n            // received_encoder_wheel_velocities_[3] = serial_pack_.rx.data.fp32_buffer[3];  // 电机 3 速度\n\n            // // 存储电机位置数据\n            // received_encoder_wheel_angle_[0] = serial_pack_.rx.data.fp32_buffer[4];  // 电机 0 位置\n            // received_encoder_wheel_angle_[1] = serial_pack_.rx.data.fp32_buffer[5];  // 电机 1 位置\n            // received_encoder_wheel_angle_[2] = serial_pack_.rx.data.fp32_buffer[6];  // 电机 2 位置\n            // received_encoder_wheel_angle_[3] = serial_pack_.rx.data.fp32_buffer[7];  // 电机 3 位置\n\n            // vx = serial_pack_.rx.data.fp32_buffer[8];\n            // vy = serial_pack_.rx.data.fp32_buffer[9];\n            // vw = serial_pack_.rx.data.fp32_buffer[10];\n            // yaw_ = serial_pack_.rx.data.fp32_buffer[11];\n            // dt_ = serial_pack_.rx.data.fp32_buffer[12];\n            // x_position_ = serial_pack_.rx.data.fp32_buffer[13];\n            // y_position_ = serial_pack_.rx.data.fp32_buffer[14];\n\n            vx = serial_pack_.rx.data.fp32_buffer[0];\n            vy = serial_pack_.rx.data.fp32_buffer[1];\n            vw = serial_pack_.rx.data.fp32_buffer[2];\n            yaw_ = serial_pack_.rx.data.fp32_buffer[3];\n            dt_ = serial_pack_.rx.data.fp32_buffer[4];\n            x_position_ = serial_pack_.rx.data.fp32_buffer[5];\n            y_position_ = serial_pack_.rx.data.fp32_buffer[6];\n\n            // 打印电机速度和位置（角度）\n            for (int i = 0; i \u003C 4; ++i) \n            {\n                // RCLCPP_DEBUG(this->get_logger(), \"%d号电机的速度: %.6f RPM, 位置: %.6f (2000pc)\",\n                //             i, received_encoder_wheel_velocities_[i], received_encoder_wheel_angle_[i]);\n\n                RCLCPP_DEBUG(this->get_logger(),\"线速度:x:%.6f,y:%.6f,z:%.6f\",vx,vy,0.0f);\n                RCLCPP_DEBUG(this->get_logger(),\"角速度:x:%.6f,y:%.6f,z:%.6f\",0.0f,0.0f,vw);\n                RCLCPP_DEBUG(this->get_logger(),\"欧拉角(逆正顺负):r:%.6f,p:%.6f,y:%.6f\",0.0f,0.0f,yaw_);\n                RCLCPP_DEBUG(this->get_logger(),\"积分间隔:%.6f\",dt_);\n                RCLCPP_DEBUG(this->get_logger(),\"右手坐标系X坐标(前正后负):%.6f\",x_position_);\n                RCLCPP_DEBUG(this->get_logger(),\"右手坐标系Y坐标(左正右负):%.6f\",y_position_);\n            }\n\n            //时间戳\n            odom_msg.header.stamp = this->get_clock()->now();\n\n            //位姿信息所参考的坐标系\n            odom_msg.header.frame_id = \"odom\";\n\n            // 设置child_frame_id（底盘坐标系）\n            odom_msg.child_frame_id = \"base_link\";  // 设置子坐标系为机器人底盘坐标系\n\n            //DOF平动位置\n            odom_msg.pose.pose.position.x = x_position_;\n            odom_msg.pose.pose.position.y = y_position_;\n            odom_msg.pose.pose.position.z = 0.0;\n\n            //从欧拉角转换为四元数\n            tf2::Quaternion q;\n            q.setRPY(0,0,yaw_);\n            //DOF转动（四元数）\n            odom_msg.pose.pose.orientation.x = q.x();\n            odom_msg.pose.pose.orientation.y = q.y();\n            odom_msg.pose.pose.orientation.z = q.z();\n            odom_msg.pose.pose.orientation.w = q.w();\n\n            //线速度\n            odom_msg.twist.twist.linear.x = vx;\n            odom_msg.twist.twist.linear.y = vy;\n            odom_msg.twist.twist.linear.z = 0.0;\n\n            //角速度\n            odom_msg.twist.twist.angular.x = 0.0;\n            odom_msg.twist.twist.angular.y = 0.0;\n            odom_msg.twist.twist.angular.z = vw;\n\n            // 发布消息\n            odom_pub_->publish(odom_msg);\n\n            // 打印位置（XYZ）\n            RCLCPP_DEBUG(\n              this->get_logger(),\n              \"位置Position(XYZ): %.6f, %.6f, %.6f\",\n              odom_msg.pose.pose.position.x,\n              odom_msg.pose.pose.position.y,\n              odom_msg.pose.pose.position.z\n            );\n\n            // 打印姿态（四元数WXYZ）\n            RCLCPP_DEBUG(\n              this->get_logger(),\n              \"姿态Orientation(WXYZ): %.6f, %.6f, %.6f, %.6f\",\n              odom_msg.pose.pose.orientation.w,  // 注意顺序：WXYZ\n              odom_msg.pose.pose.orientation.x,\n              odom_msg.pose.pose.orientation.y,\n              odom_msg.pose.pose.orientation.z\n            );\n\n            // 打印姿态（欧拉角RPY）\n            RCLCPP_DEBUG(\n              this->get_logger(),\n              \"欧拉角Euler(RPY): %.6f, %.6f, %.6f\",\n              0.0,\n              0.0,\n              yaw_\n            );\n\n            // 打印线速度（XYZ）\n            RCLCPP_DEBUG(\n              this->get_logger(),\n              \"线速度LinearVel(XYZ): %.6f, %.6f, %.6f\",\n              odom_msg.twist.twist.linear.x,\n              odom_msg.twist.twist.linear.y,\n              odom_msg.twist.twist.linear.z\n            );\n\n            // 打印角速度（XYZ）\n            RCLCPP_DEBUG(\n              this->get_logger(),\n              \"角速度AngularVel(XYZ): %.6f, %.6f, %.6f\",\n              odom_msg.twist.twist.angular.x,\n              odom_msg.twist.twist.angular.y,\n              odom_msg.twist.twist.angular.z\n            );\n\n            /* 接下来发布tf */\n            auto transform = geometry_msgs::msg::TransformStamped();\n            transform.header.stamp = odom_msg.header.stamp; // 时间戳与odom同步\n            transform.header.frame_id = \"odom\";\n            transform.child_frame_id = \"base_link\";\n            transform.transform.translation.x = x_position_;\n            transform.transform.translation.y = y_position_;\n            transform.transform.translation.z = 0.0;\n            transform.transform.rotation = odom_msg.pose.pose.orientation; // 复用四元数\n            tf_broadcaster_->sendTransform(transform);\n          }\n\n        }\n        // 继续监听新的数据\n        async_receive_message();\n    }\n    );\n  }\n\n  void cmd_vel_sub_callback(const geometry_msgs::msg::Twist &cmd_msg)\n  {\n    // bool bool_buffer[] = {1, 0, 1, 0};\n    // int8_t int8_buffer[] = {0x11,0x22};\n    // int16_t int16_buffer[] = {2000,6666};\n    // int32_t int32_buffer[] = {305419896};\n    fp32 fp32_buffer[] = {(fp32)cmd_msg.linear.x,(fp32)cmd_msg.linear.y,(fp32)cmd_msg.linear.z,\n                          (fp32)cmd_msg.angular.x,(fp32)cmd_msg.angular.y,(fp32)cmd_msg.angular.z};\n\n    //由于ROS2中node为局部变量，所以只能在node中调用send函数，所以Serial_Transmit只负责处理data_buffer。\n    serial_pack_.tx.Data_Pack(0x01, \n                                 nullptr, 0,\n                                 nullptr, 0,\n                                 nullptr, 0,\n                                 nullptr, 0,\n                                 fp32_buffer, sizeof(fp32_buffer) / sizeof(fp32));\n\n    auto port = serial_driver_->port();\n\n    try\n    {\n      size_t bytes_size = port->send(tx_data_buffer);\n\n      RCLCPP_DEBUG(this->get_logger(), \"平动XYZ：%.6f,%.6f,%.6f\", fp32_buffer[0],fp32_buffer[1],fp32_buffer[2]);\n      RCLCPP_DEBUG(this->get_logger(), \"转动XYZ：%.6f,%.6f,%.6f\", fp32_buffer[3],fp32_buffer[4],fp32_buffer[5]);\n      RCLCPP_DEBUG(this->get_logger(), \"(%ld bytes)\", bytes_size);\n    }\n    catch(const std::exception &ex)\n    {\n      RCLCPP_ERROR(this->get_logger(), \"Error Transmiting from serial port:%s\",ex.what());\n    }\n  }\n};\n\nint main(int argc, char **argv)\n{\n  rclcpp::init(argc, argv);\n\n  auto node = std::make_shared\u003COdom_KC_Node>();\n\n  rclcpp::spin(node);\n\n  rclcpp::shutdown();\n  return 0;\n}\n",[29,1331,1332,1339,1346,1353,1360,1367,1374,1381,1388,1395,1402,1409,1413,1429,1433,1454,1458,1463,1481,1486,1491,1496,1520,1525,1545,1549,1554,1586,1615,1619,1641,1646,1651,1670,1675,1692,1707,1723,1727,1732,1737,1741,1777,1782,1815,1826,1841,1845,1865,1912,1942,1946,1967,1971,2001,2009,2014,2019,2025,2066,2071,2077,2110,2115,2121,2191,2196,2208,2214,2219,2225,2250,2273,2278,2305,2325,2330,2356,2361,2370,2376,2384,2390,2395,2403,2409,2414,2420,2440,2458,2476,2481,2508,2524,2530,2535,2550,2565,2570,2585,2590,2608,2613,2619,2673,2678,2684,2707,2712,2727,2733,2765,2771,2785,2791,2802,2811,2823,2831,2838,2845,2852,2860,2866,2872,2891,2896,2921,2941,2946,2955,2964,2973,2982,2991,2996,3004,3013,3022,3031,3040,3045,3051,3057,3063,3069,3075,3081,3087,3092,3108,3122,3136,3150,3165,3180,3195,3200,3206,3238,3244,3250,3256,3261,3305,3350,3396,3421,3446,3471,3477,3482,3488,3510,3515,3521,3534,3539,3545,3561,3566,3572,3583,3594,3606,3611,3617,3626,3645,3651,3667,3682,3697,3712,3717,3723,3734,3745,3757,3762,3768,3780,3792,3803,3808,3814,3826,3831,3837,3844,3857,3877,3883,3889,3895,3901,3906,3912,3919,3930,3954,3963,3969,3975,3981,3986,3991,3997,4004,4015,4035,4043,4050,4056,4061,4066,4072,4079,4090,4110,4116,4122,4128,4133,4138,4144,4151,4162,4182,4188,4194,4200,4205,4210,4216,4240,4254,4266,4278,4288,4298,4310,4324,4336,4341,4346,4352,4358,4366,4371,4377,4382,4387,4416,4421,4427,4433,4439,4445,4456,4462,4467,4473,4492,4504,4515,4526,4537,4557,4562,4577,4582,4587,4592,4612,4617,4665,4709,4736,4741,4758,4763,4792,4797,4802,4808,4813,4841,4846,4859,4864,4888,4893,4906,4911,4923,4933],{"__ignoreMap":27},[32,1333,1334,1336],{"class":34,"line":35},[32,1335,648],{"class":647},[32,1337,1338],{"class":42}," \"rclcpp/rclcpp.hpp\"\n",[32,1340,1341,1343],{"class":34,"line":274},[32,1342,648],{"class":647},[32,1344,1345],{"class":42}," \"serial_driver/serial_driver.hpp\"\n",[32,1347,1348,1350],{"class":34,"line":286},[32,1349,648],{"class":647},[32,1351,1352],{"class":42}," \u003Cchrono>\n",[32,1354,1355,1357],{"class":34,"line":295},[32,1356,648],{"class":647},[32,1358,1359],{"class":42}," \u003Cfunctional>\n",[32,1361,1362,1364],{"class":34,"line":304},[32,1363,648],{"class":647},[32,1365,1366],{"class":42}," \u003Crclcpp/logging.hpp>\n",[32,1368,1369,1371],{"class":34,"line":316},[32,1370,648],{"class":647},[32,1372,1373],{"class":42}," \u003Cstring>\n",[32,1375,1376,1378],{"class":34,"line":322},[32,1377,648],{"class":647},[32,1379,1380],{"class":42}," \"sensor04_odom_kc/serial_pack.h\"\n",[32,1382,1383,1385],{"class":34,"line":331},[32,1384,648],{"class":647},[32,1386,1387],{"class":42}," \"geometry_msgs/msg/twist.hpp\"\n",[32,1389,1390,1392],{"class":34,"line":337},[32,1391,648],{"class":647},[32,1393,1394],{"class":42}," \"nav_msgs/msg/odometry.hpp\"\n",[32,1396,1397,1399],{"class":34,"line":346},[32,1398,648],{"class":647},[32,1400,1401],{"class":42}," \u003Ctf2/LinearMath/Quaternion.h>\n",[32,1403,1404,1406],{"class":34,"line":354},[32,1405,648],{"class":647},[32,1407,1408],{"class":42}," \u003Ctf2_ros/transform_broadcaster.h>\n",[32,1410,1411],{"class":34,"line":366},[32,1412,676],{"emptyLinePlaceholder":675},[32,1414,1415,1417,1420,1423,1426],{"class":34,"line":375},[32,1416,710],{"class":647},[32,1418,1419],{"class":38}," std",[32,1421,1422],{"class":681},"::vector",[32,1424,1425],{"class":647},"\u003Cuint8_t>",[32,1427,1428],{"class":681}," tx_data_buffer;\n",[32,1430,1431],{"class":34,"line":383},[32,1432,676],{"emptyLinePlaceholder":675},[32,1434,1435,1438,1441,1444,1447,1449,1451],{"class":34,"line":391},[32,1436,1437],{"class":647},"class",[32,1439,1440],{"class":38}," Odom_KC_Node",[32,1442,1443],{"class":681}," : ",[32,1445,1446],{"class":647},"public",[32,1448,62],{"class":38},[32,1450,752],{"class":681},[32,1452,1453],{"class":38},"Node\n",[32,1455,1456],{"class":34,"line":403},[32,1457,772],{"class":681},[32,1459,1460],{"class":34,"line":414},[32,1461,1462],{"class":647},"public:\n",[32,1464,1465,1468,1471,1474,1476,1479],{"class":34,"line":424},[32,1466,1467],{"class":38},"  Odom_KC_Node",[32,1469,1470],{"class":681},"() : ",[32,1472,1473],{"class":38},"Node",[32,1475,573],{"class":681},[32,1477,1478],{"class":42},"\"Odom_KC_Node\"",[32,1480,767],{"class":681},[32,1482,1483],{"class":34,"line":434},[32,1484,1485],{"class":681},"  {\n",[32,1487,1488],{"class":34,"line":445},[32,1489,1490],{"class":270},"    // 声明参数（带默认值）\n",[32,1492,1493],{"class":34,"line":454},[32,1494,1495],{"class":270},"    // 串口设备默认名（根据实际设备调整）\n",[32,1497,1498,1500,1503,1506,1508,1511,1514,1517],{"class":34,"line":460},[32,1499,894],{"class":52},[32,1501,1502],{"class":681},"->",[32,1504,1505],{"class":38},"declare_parameter",[32,1507,573],{"class":681},[32,1509,1510],{"class":42},"\"device_name\"",[32,1512,1513],{"class":681},", ",[32,1515,1516],{"class":42},"\"/dev/ttyACM1\"",[32,1518,1519],{"class":681},");\n",[32,1521,1522],{"class":34,"line":469},[32,1523,1524],{"class":270},"    //串口默认波特率\n",[32,1526,1527,1529,1531,1533,1535,1538,1540,1543],{"class":34,"line":477},[32,1528,894],{"class":52},[32,1530,1502],{"class":681},[32,1532,1505],{"class":38},[32,1534,573],{"class":681},[32,1536,1537],{"class":42},"\"baud_rate\"",[32,1539,1513],{"class":681},[32,1541,1542],{"class":52},"115200",[32,1544,1519],{"class":681},[32,1546,1547],{"class":34,"line":489},[32,1548,676],{"emptyLinePlaceholder":675},[32,1550,1551],{"class":34,"line":496},[32,1552,1553],{"class":270},"    // 获取参数值\n",[32,1555,1556,1559,1561,1564,1566,1568,1570,1573,1575,1577,1580,1583],{"class":34,"line":503},[32,1557,1558],{"class":647},"    const",[32,1560,1419],{"class":38},[32,1562,1563],{"class":681},"::string device_name ",[32,1565,783],{"class":647},[32,1567,1111],{"class":52},[32,1569,1502],{"class":681},[32,1571,1572],{"class":38},"get_parameter",[32,1574,573],{"class":681},[32,1576,1510],{"class":42},[32,1578,1579],{"class":681},").",[32,1581,1582],{"class":38},"as_string",[32,1584,1585],{"class":681},"();\n",[32,1587,1588,1590,1593,1596,1598,1600,1602,1604,1606,1608,1610,1613],{"class":34,"line":510},[32,1589,1558],{"class":647},[32,1591,1592],{"class":647}," uint32_t",[32,1594,1595],{"class":681}," baud_rate ",[32,1597,783],{"class":647},[32,1599,1111],{"class":52},[32,1601,1502],{"class":681},[32,1603,1572],{"class":38},[32,1605,573],{"class":681},[32,1607,1537],{"class":42},[32,1609,1579],{"class":681},[32,1611,1612],{"class":38},"as_int",[32,1614,1585],{"class":681},[32,1616,1617],{"class":34,"line":521},[32,1618,676],{"emptyLinePlaceholder":675},[32,1620,1621,1624,1626,1628,1630,1633,1636,1639],{"class":34,"line":528},[32,1622,1623],{"class":38},"    RCLCPP_INFO",[32,1625,573],{"class":681},[32,1627,1123],{"class":52},[32,1629,1502],{"class":681},[32,1631,1632],{"class":38},"get_logger",[32,1634,1635],{"class":681},"(), ",[32,1637,1638],{"class":42},"\"Serial port Node Open!\"",[32,1640,1519],{"class":681},[32,1642,1643],{"class":34,"line":535},[32,1644,1645],{"class":270},"    // 创建串口配置对象\n",[32,1647,1648],{"class":34,"line":542},[32,1649,1650],{"class":270},"    // 波特率默认115200；不开启流控制；无奇偶效验；停止位1。\n",[32,1652,1653,1656,1658,1661,1664,1667],{"class":34,"line":1012},[32,1654,1655],{"class":38},"    drivers",[32,1657,752],{"class":681},[32,1659,1660],{"class":38},"serial_driver",[32,1662,1663],{"class":681},"::SerialPortConfig ",[32,1665,1666],{"class":38},"config",[32,1668,1669],{"class":681},"(\n",[32,1671,1672],{"class":34,"line":1077},[32,1673,1674],{"class":681},"        baud_rate,\n",[32,1676,1677,1680,1682,1684,1686,1689],{"class":34,"line":1082},[32,1678,1679],{"class":38},"        drivers",[32,1681,752],{"class":681},[32,1683,1660],{"class":38},[32,1685,752],{"class":681},[32,1687,1688],{"class":38},"FlowControl",[32,1690,1691],{"class":681},"::NONE,\n",[32,1693,1694,1696,1698,1700,1702,1705],{"class":34,"line":1088},[32,1695,1679],{"class":38},[32,1697,752],{"class":681},[32,1699,1660],{"class":38},[32,1701,752],{"class":681},[32,1703,1704],{"class":38},"Parity",[32,1706,1691],{"class":681},[32,1708,1709,1711,1713,1715,1717,1720],{"class":34,"line":1094},[32,1710,1679],{"class":38},[32,1712,752],{"class":681},[32,1714,1660],{"class":38},[32,1716,752],{"class":681},[32,1718,1719],{"class":38},"StopBits",[32,1721,1722],{"class":681},"::ONE);\n",[32,1724,1725],{"class":34,"line":1100},[32,1726,676],{"emptyLinePlaceholder":675},[32,1728,1729],{"class":34,"line":1136},[32,1730,1731],{"class":270},"    // 初始化串口\n",[32,1733,1734],{"class":34,"line":1168},[32,1735,1736],{"class":647},"    try\n",[32,1738,1739],{"class":34,"line":1188},[32,1740,826],{"class":681},[32,1742,1743,1746,1748,1750,1752,1755,1757,1760,1762,1765,1767,1770,1773,1775],{"class":34,"line":1193},[32,1744,1745],{"class":681},"      io_context_ ",[32,1747,783],{"class":647},[32,1749,1419],{"class":38},[32,1751,752],{"class":681},[32,1753,1754],{"class":38},"make_shared",[32,1756,810],{"class":681},[32,1758,1759],{"class":38},"drivers",[32,1761,752],{"class":681},[32,1763,1764],{"class":38},"common",[32,1766,752],{"class":681},[32,1768,1769],{"class":38},"IoContext",[32,1771,1772],{"class":681},">(",[32,1774,917],{"class":52},[32,1776,1519],{"class":681},[32,1778,1779],{"class":34,"line":1199},[32,1780,1781],{"class":270},"      // 初始化 serial_driver_\n",[32,1783,1784,1787,1789,1791,1793,1795,1797,1799,1801,1803,1805,1808,1810,1812],{"class":34,"line":1221},[32,1785,1786],{"class":681},"      serial_driver_ ",[32,1788,783],{"class":647},[32,1790,1419],{"class":38},[32,1792,752],{"class":681},[32,1794,1754],{"class":38},[32,1796,810],{"class":681},[32,1798,1759],{"class":38},[32,1800,752],{"class":681},[32,1802,1660],{"class":38},[32,1804,752],{"class":681},[32,1806,1807],{"class":38},"SerialDriver",[32,1809,1772],{"class":681},[32,1811,844],{"class":647},[32,1813,1814],{"class":681},"io_context_);\n",[32,1816,1817,1820,1823],{"class":34,"line":1226},[32,1818,1819],{"class":681},"      serial_driver_->",[32,1821,1822],{"class":38},"init_port",[32,1824,1825],{"class":681},"(device_name, config);\n",[32,1827,1828,1830,1833,1836,1839],{"class":34,"line":1247},[32,1829,1819],{"class":681},[32,1831,1832],{"class":38},"port",[32,1834,1835],{"class":681},"()->",[32,1837,1838],{"class":38},"open",[32,1840,1585],{"class":681},[32,1842,1843],{"class":34,"line":1252},[32,1844,676],{"emptyLinePlaceholder":675},[32,1846,1847,1850,1852,1854,1856,1858,1860,1863],{"class":34,"line":1275},[32,1848,1849],{"class":38},"      RCLCPP_INFO",[32,1851,573],{"class":681},[32,1853,1123],{"class":52},[32,1855,1502],{"class":681},[32,1857,1632],{"class":38},[32,1859,1635],{"class":681},[32,1861,1862],{"class":42},"\"Serial port initialized successfully\"",[32,1864,1519],{"class":681},[32,1866,1867,1869,1871,1873,1875,1877,1879,1882,1885,1888,1891,1893,1896,1899,1901,1904,1906,1909],{"class":34,"line":1280},[32,1868,1849],{"class":38},[32,1870,573],{"class":681},[32,1872,1123],{"class":52},[32,1874,1502],{"class":681},[32,1876,1632],{"class":38},[32,1878,1635],{"class":681},[32,1880,1881],{"class":42},"\"Using device: ",[32,1883,1884],{"class":52},"%s",[32,1886,1887],{"class":42},"\"",[32,1889,1890],{"class":681},", serial_driver_->",[32,1892,1832],{"class":38},[32,1894,1895],{"class":681},"().",[32,1897,1898],{"class":38},"get",[32,1900,1835],{"class":681},[32,1902,1903],{"class":38},"device_name",[32,1905,1895],{"class":681},[32,1907,1908],{"class":38},"c_str",[32,1910,1911],{"class":681},"());\n",[32,1913,1914,1916,1918,1920,1922,1924,1926,1929,1932,1934,1937,1940],{"class":34,"line":1299},[32,1915,1849],{"class":38},[32,1917,573],{"class":681},[32,1919,1123],{"class":52},[32,1921,1502],{"class":681},[32,1923,1632],{"class":38},[32,1925,1635],{"class":681},[32,1927,1928],{"class":42},"\"Baud_rate: ",[32,1930,1931],{"class":52},"%d",[32,1933,1887],{"class":42},[32,1935,1936],{"class":681},", config.",[32,1938,1939],{"class":38},"get_baud_rate",[32,1941,1911],{"class":681},[32,1943,1944],{"class":34,"line":1304},[32,1945,879],{"class":681},[32,1947,1948,1951,1953,1956,1958,1961,1964],{"class":34,"line":1309},[32,1949,1950],{"class":647},"    catch",[32,1952,1022],{"class":681},[32,1954,1955],{"class":647},"const",[32,1957,1419],{"class":38},[32,1959,1960],{"class":681},"::exception ",[32,1962,1963],{"class":647},"&",[32,1965,1966],{"class":681},"ex)\n",[32,1968,1969],{"class":34,"line":1315},[32,1970,826],{"class":681},[32,1972,1973,1976,1978,1980,1982,1984,1986,1989,1991,1993,1996,1999],{"class":34,"line":1320},[32,1974,1975],{"class":38},"      RCLCPP_ERROR",[32,1977,573],{"class":681},[32,1979,1123],{"class":52},[32,1981,1502],{"class":681},[32,1983,1632],{"class":38},[32,1985,1635],{"class":681},[32,1987,1988],{"class":42},"\"Failed to initialize serial port: ",[32,1990,1884],{"class":52},[32,1992,1887],{"class":42},[32,1994,1995],{"class":681},", ex.",[32,1997,1998],{"class":38},"what",[32,2000,1911],{"class":681},[32,2002,2004,2007],{"class":34,"line":2003},55,[32,2005,2006],{"class":647},"      return",[32,2008,874],{"class":681},[32,2010,2012],{"class":34,"line":2011},56,[32,2013,879],{"class":681},[32,2015,2017],{"class":34,"line":2016},57,[32,2018,676],{"emptyLinePlaceholder":675},[32,2020,2022],{"class":34,"line":2021},58,[32,2023,2024],{"class":270},"    //串口发布方\n",[32,2026,2028,2031,2033,2035,2038,2040,2043,2045,2048,2051,2053,2055,2058,2061,2064],{"class":34,"line":2027},59,[32,2029,2030],{"class":681},"    odom_pub_ ",[32,2032,783],{"class":647},[32,2034,1111],{"class":52},[32,2036,2037],{"class":681},"->create_publisher",[32,2039,810],{"class":647},[32,2041,2042],{"class":38},"nav_msgs",[32,2044,752],{"class":681},[32,2046,2047],{"class":38},"msg",[32,2049,2050],{"class":681},"::Odometry",[32,2052,1211],{"class":647},[32,2054,573],{"class":681},[32,2056,2057],{"class":42},"\"odom\"",[32,2059,2060],{"class":681},",",[32,2062,2063],{"class":52},"10",[32,2065,1519],{"class":681},[32,2067,2069],{"class":34,"line":2068},60,[32,2070,676],{"emptyLinePlaceholder":675},[32,2072,2074],{"class":34,"line":2073},61,[32,2075,2076],{"class":270},"    //初始化tf广播器\n",[32,2078,2080,2083,2085,2087,2089,2092,2094,2097,2099,2102,2104,2106,2108],{"class":34,"line":2079},62,[32,2081,2082],{"class":681},"    tf_broadcaster_ ",[32,2084,783],{"class":647},[32,2086,1419],{"class":38},[32,2088,752],{"class":681},[32,2090,2091],{"class":38},"make_unique",[32,2093,810],{"class":681},[32,2095,2096],{"class":38},"tf2_ros",[32,2098,752],{"class":681},[32,2100,2101],{"class":38},"TransformBroadcaster",[32,2103,1772],{"class":681},[32,2105,844],{"class":647},[32,2107,1123],{"class":52},[32,2109,1519],{"class":681},[32,2111,2113],{"class":34,"line":2112},63,[32,2114,676],{"emptyLinePlaceholder":675},[32,2116,2118],{"class":34,"line":2117},64,[32,2119,2120],{"class":270},"    //初始化cmd_vel消息订阅\n",[32,2122,2124,2127,2129,2131,2134,2136,2139,2141,2143,2146,2148,2150,2153,2155,2157,2159,2162,2164,2167,2169,2171,2174,2177,2179,2181,2183,2185,2188],{"class":34,"line":2123},65,[32,2125,2126],{"class":681},"    cmd_vel_sub_ ",[32,2128,783],{"class":647},[32,2130,1111],{"class":52},[32,2132,2133],{"class":681},"->create_subscription",[32,2135,810],{"class":647},[32,2137,2138],{"class":38},"geometry_msgs",[32,2140,752],{"class":681},[32,2142,2047],{"class":38},[32,2144,2145],{"class":681},"::Twist",[32,2147,1211],{"class":647},[32,2149,573],{"class":681},[32,2151,2152],{"class":42},"\"cmd_vel\"",[32,2154,2060],{"class":681},[32,2156,2063],{"class":52},[32,2158,2060],{"class":681},[32,2160,2161],{"class":38},"std",[32,2163,752],{"class":681},[32,2165,2166],{"class":38},"bind",[32,2168,573],{"class":681},[32,2170,1963],{"class":647},[32,2172,2173],{"class":38},"Odom_KC_Node",[32,2175,2176],{"class":681},"::cmd_vel_sub_callback,",[32,2178,1123],{"class":52},[32,2180,2060],{"class":681},[32,2182,2161],{"class":38},[32,2184,752],{"class":681},[32,2186,2187],{"class":38},"placeholders",[32,2189,2190],{"class":681},"::_1));\n",[32,2192,2194],{"class":34,"line":2193},66,[32,2195,676],{"emptyLinePlaceholder":675},[32,2197,2199,2202,2205],{"class":34,"line":2198},67,[32,2200,2201],{"class":38},"    async_receive_message",[32,2203,2204],{"class":681},"();",[32,2206,2207],{"class":270},"   //进入异步接收\n",[32,2209,2211],{"class":34,"line":2210},68,[32,2212,2213],{"class":681},"  }\n",[32,2215,2217],{"class":34,"line":2216},69,[32,2218,676],{"emptyLinePlaceholder":675},[32,2220,2222],{"class":34,"line":2221},70,[32,2223,2224],{"class":647},"private:\n",[32,2226,2228,2231,2234,2236,2238,2240,2242,2245,2247],{"class":34,"line":2227},71,[32,2229,2230],{"class":38},"  std",[32,2232,2233],{"class":681},"::shared_ptr",[32,2235,810],{"class":647},[32,2237,1759],{"class":38},[32,2239,752],{"class":681},[32,2241,1660],{"class":38},[32,2243,2244],{"class":681},"::SerialDriver",[32,2246,1211],{"class":647},[32,2248,2249],{"class":681}," serial_driver_;\n",[32,2251,2253,2255,2257,2259,2261,2263,2265,2268,2270],{"class":34,"line":2252},72,[32,2254,2230],{"class":38},[32,2256,2233],{"class":681},[32,2258,810],{"class":647},[32,2260,1759],{"class":38},[32,2262,752],{"class":681},[32,2264,1764],{"class":38},[32,2266,2267],{"class":681},"::IoContext",[32,2269,1211],{"class":647},[32,2271,2272],{"class":681}," io_context_;\n",[32,2274,2276],{"class":34,"line":2275},73,[32,2277,676],{"emptyLinePlaceholder":675},[32,2279,2281,2284,2286,2289,2291,2293,2295,2297,2299,2302],{"class":34,"line":2280},74,[32,2282,2283],{"class":38},"  rclcpp",[32,2285,752],{"class":681},[32,2287,2288],{"class":38},"Publisher",[32,2290,810],{"class":681},[32,2292,2042],{"class":38},[32,2294,752],{"class":681},[32,2296,2047],{"class":38},[32,2298,752],{"class":681},[32,2300,2301],{"class":38},"Odometry",[32,2303,2304],{"class":681},">::SharedPtr odom_pub_;\n",[32,2306,2308,2310,2313,2315,2317,2320,2322],{"class":34,"line":2307},75,[32,2309,2230],{"class":38},[32,2311,2312],{"class":681},"::unique_ptr",[32,2314,810],{"class":647},[32,2316,2096],{"class":38},[32,2318,2319],{"class":681},"::TransformBroadcaster",[32,2321,1211],{"class":647},[32,2323,2324],{"class":681}," tf_broadcaster_;\n",[32,2326,2328],{"class":34,"line":2327},76,[32,2329,676],{"emptyLinePlaceholder":675},[32,2331,2333,2335,2337,2340,2342,2344,2346,2348,2350,2353],{"class":34,"line":2332},77,[32,2334,2283],{"class":38},[32,2336,752],{"class":681},[32,2338,2339],{"class":38},"Subscription",[32,2341,810],{"class":681},[32,2343,2138],{"class":38},[32,2345,752],{"class":681},[32,2347,2047],{"class":38},[32,2349,752],{"class":681},[32,2351,2352],{"class":38},"Twist",[32,2354,2355],{"class":681},">::SharedPtr cmd_vel_sub_;\n",[32,2357,2359],{"class":34,"line":2358},78,[32,2360,676],{"emptyLinePlaceholder":675},[32,2362,2364,2367],{"class":34,"line":2363},79,[32,2365,2366],{"class":270},"  //",[32,2368,2369],{"class":270}," // 四个轮子的速度(对应单片机的顺序)（单位：rpm）\n",[32,2371,2373],{"class":34,"line":2372},80,[32,2374,2375],{"class":270},"  // fp32 received_encoder_wheel_velocities_[4] = {0.0, 0.0, 0.0, 0.0};\n",[32,2377,2379,2381],{"class":34,"line":2378},81,[32,2380,2366],{"class":270},[32,2382,2383],{"class":270}," // 四个轮子的速度(对应单片机的顺序)（单位：m/s）\n",[32,2385,2387],{"class":34,"line":2386},82,[32,2388,2389],{"class":270},"  // fp32 encoder_wheel_velocities_[4] = {0.0, 0.0, 0.0, 0.0};  \n",[32,2391,2393],{"class":34,"line":2392},83,[32,2394,676],{"emptyLinePlaceholder":675},[32,2396,2398,2400],{"class":34,"line":2397},84,[32,2399,2366],{"class":270},[32,2401,2402],{"class":270}," // 四个轮子的速度(对应单片机的顺序)（单位：2000pc）\n",[32,2404,2406],{"class":34,"line":2405},85,[32,2407,2408],{"class":270},"  // fp32 received_encoder_wheel_angle_[4] = {0.0, 0.0, 0.0, 0.0};\n",[32,2410,2412],{"class":34,"line":2411},86,[32,2413,676],{"emptyLinePlaceholder":675},[32,2415,2417],{"class":34,"line":2416},87,[32,2418,2419],{"class":270},"  //麦克纳姆轮底盘参数\n",[32,2421,2423,2426,2429,2431,2434,2437],{"class":34,"line":2422},88,[32,2424,2425],{"class":647},"  const",[32,2427,2428],{"class":681}," fp32 wheel_spacing ",[32,2430,783],{"class":647},[32,2432,2433],{"class":52}," 0.093",[32,2435,2436],{"class":681},";",[32,2438,2439],{"class":270}," // 轮间距（单位：米）\n",[32,2441,2443,2445,2448,2450,2453,2455],{"class":34,"line":2442},89,[32,2444,2425],{"class":647},[32,2446,2447],{"class":681}," fp32 alex_spacing ",[32,2449,783],{"class":647},[32,2451,2452],{"class":52}," 0.085",[32,2454,2436],{"class":681},[32,2456,2457],{"class":270}," // 轮距（单位：米）\n",[32,2459,2461,2463,2466,2468,2471,2473],{"class":34,"line":2460},90,[32,2462,2425],{"class":647},[32,2464,2465],{"class":681}," fp32 wheel_radius_ ",[32,2467,783],{"class":647},[32,2469,2470],{"class":52}," 0.0375",[32,2472,2436],{"class":681},[32,2474,2475],{"class":270}," // 轮子半径（单位：米）\n",[32,2477,2479],{"class":34,"line":2478},91,[32,2480,676],{"emptyLinePlaceholder":675},[32,2482,2484,2487,2489,2492,2495,2497,2499,2502,2504,2506],{"class":34,"line":2483},92,[32,2485,2486],{"class":681},"  fp32 vy",[32,2488,783],{"class":647},[32,2490,2491],{"class":52},"0.0",[32,2493,2494],{"class":681},",vx",[32,2496,783],{"class":647},[32,2498,2491],{"class":52},[32,2500,2501],{"class":681},",vw",[32,2503,783],{"class":647},[32,2505,2491],{"class":52},[32,2507,874],{"class":681},[32,2509,2511,2514,2516,2519,2521],{"class":34,"line":2510},93,[32,2512,2513],{"class":681},"  fp32 yaw_ ",[32,2515,783],{"class":647},[32,2517,2518],{"class":52}," 0.0",[32,2520,2436],{"class":681},[32,2522,2523],{"class":270}," // 机器人航向角（单位：弧度）\n",[32,2525,2527],{"class":34,"line":2526},94,[32,2528,2529],{"class":681},"  fp32 dt_;\n",[32,2531,2533],{"class":34,"line":2532},95,[32,2534,676],{"emptyLinePlaceholder":675},[32,2536,2538,2541,2543,2545,2547],{"class":34,"line":2537},96,[32,2539,2540],{"class":681},"  fp32 x_position_ ",[32,2542,783],{"class":647},[32,2544,2518],{"class":52},[32,2546,2436],{"class":681},[32,2548,2549],{"class":270}," // x位置\n",[32,2551,2553,2556,2558,2560,2562],{"class":34,"line":2552},97,[32,2554,2555],{"class":681},"  fp32 y_position_ ",[32,2557,783],{"class":647},[32,2559,2518],{"class":52},[32,2561,2436],{"class":681},[32,2563,2564],{"class":270}," // y位置\n",[32,2566,2568],{"class":34,"line":2567},98,[32,2569,676],{"emptyLinePlaceholder":675},[32,2571,2573,2576,2579,2582],{"class":34,"line":2572},99,[32,2574,2575],{"class":647},"  void",[32,2577,2578],{"class":38}," async_receive_message",[32,2580,2581],{"class":681},"()",[32,2583,2584],{"class":270},"  //创建一个函数更方便重新调用\n",[32,2586,2588],{"class":34,"line":2587},100,[32,2589,1485],{"class":681},[32,2591,2593,2596,2599,2601,2604,2606],{"class":34,"line":2592},101,[32,2594,2595],{"class":647},"    auto",[32,2597,2598],{"class":681}," port ",[32,2600,783],{"class":647},[32,2602,2603],{"class":681}," serial_driver_->",[32,2605,1832],{"class":38},[32,2607,1585],{"class":681},[32,2609,2611],{"class":34,"line":2610},102,[32,2612,676],{"emptyLinePlaceholder":675},[32,2614,2616],{"class":34,"line":2615},103,[32,2617,2618],{"class":270},"    // 设置接收回调函数\n",[32,2620,2622,2625,2628,2631,2633,2636,2638,2640,2642,2645,2647,2650,2653,2655,2658,2660,2662,2665,2668,2671],{"class":34,"line":2621},104,[32,2623,2624],{"class":681},"    port->",[32,2626,2627],{"class":38},"async_receive",[32,2629,2630],{"class":681},"([",[32,2632,1123],{"class":52},[32,2634,2635],{"class":681},"](",[32,2637,1955],{"class":647},[32,2639,1419],{"class":38},[32,2641,752],{"class":681},[32,2643,2644],{"class":38},"vector",[32,2646,810],{"class":681},[32,2648,2649],{"class":647},"uint8_t",[32,2651,2652],{"class":681},"> ",[32,2654,1963],{"class":647},[32,2656,2657],{"class":763},"data",[32,2659,2060],{"class":681},[32,2661,1955],{"class":647},[32,2663,2664],{"class":647}," size_t",[32,2666,2667],{"class":647}," &",[32,2669,2670],{"class":763},"size",[32,2672,1218],{"class":681},[32,2674,2676],{"class":34,"line":2675},105,[32,2677,826],{"class":681},[32,2679,2681],{"class":34,"line":2680},106,[32,2682,2683],{"class":270},"      //创建odom消息类型\n",[32,2685,2687,2690,2693,2695,2697,2699,2701,2703,2705],{"class":34,"line":2686},107,[32,2688,2689],{"class":647},"      auto",[32,2691,2692],{"class":681}," odom_msg ",[32,2694,783],{"class":647},[32,2696,65],{"class":38},[32,2698,752],{"class":681},[32,2700,2047],{"class":38},[32,2702,752],{"class":681},[32,2704,2301],{"class":38},[32,2706,1585],{"class":681},[32,2708,2710],{"class":34,"line":2709},108,[32,2711,676],{"emptyLinePlaceholder":675},[32,2713,2715,2718,2721,2723,2725],{"class":34,"line":2714},109,[32,2716,2717],{"class":647},"        if",[32,2719,2720],{"class":681}," (size ",[32,2722,1211],{"class":647},[32,2724,804],{"class":52},[32,2726,767],{"class":681},[32,2728,2730],{"class":34,"line":2729},110,[32,2731,2732],{"class":681},"        {\n",[32,2734,2736,2739,2741,2743,2745,2747,2749,2751,2753,2755,2757,2760,2762],{"class":34,"line":2735},111,[32,2737,2738],{"class":647},"          for",[32,2740,573],{"class":681},[32,2742,796],{"class":647},[32,2744,799],{"class":681},[32,2746,783],{"class":647},[32,2748,804],{"class":52},[32,2750,807],{"class":681},[32,2752,810],{"class":647},[32,2754,1022],{"class":681},[32,2756,796],{"class":647},[32,2758,2759],{"class":681},")size;",[32,2761,819],{"class":647},[32,2763,2764],{"class":681},"i)\n",[32,2766,2768],{"class":34,"line":2767},112,[32,2769,2770],{"class":681},"          {\n",[32,2772,2774,2777,2780,2782],{"class":34,"line":2773},113,[32,2775,2776],{"class":647},"            uint8_t",[32,2778,2779],{"class":681}," data_buffer ",[32,2781,783],{"class":647},[32,2783,2784],{"class":681}," data[i];\n",[32,2786,2788],{"class":34,"line":2787},114,[32,2789,2790],{"class":270},"            // 处理接收到的数据\n",[32,2792,2794,2797,2800],{"class":34,"line":2793},115,[32,2795,2796],{"class":681},"            serial_pack_.rx.",[32,2798,2799],{"class":38},"Data_Analysis",[32,2801,1669],{"class":681},[32,2803,2805,2808],{"class":34,"line":2804},116,[32,2806,2807],{"class":647},"              &",[32,2809,2810],{"class":681},"data_buffer,\n",[32,2812,2814,2817,2820],{"class":34,"line":2813},117,[32,2815,2816],{"class":647},"              0x",[32,2818,2819],{"class":52},"01",[32,2821,2822],{"class":681},",\n",[32,2824,2826,2829],{"class":34,"line":2825},118,[32,2827,2828],{"class":52},"            0",[32,2830,2822],{"class":681},[32,2832,2834,2836],{"class":34,"line":2833},119,[32,2835,2828],{"class":52},[32,2837,2822],{"class":681},[32,2839,2841,2843],{"class":34,"line":2840},120,[32,2842,2828],{"class":52},[32,2844,2822],{"class":681},[32,2846,2848,2850],{"class":34,"line":2847},121,[32,2849,2828],{"class":52},[32,2851,2822],{"class":681},[32,2853,2855,2858],{"class":34,"line":2854},122,[32,2856,2857],{"class":52},"            8",[32,2859,1519],{"class":681},[32,2861,2863],{"class":34,"line":2862},123,[32,2864,2865],{"class":681},"          }\n",[32,2867,2869],{"class":34,"line":2868},124,[32,2870,2871],{"class":270},"          //由于在ROS2中，node是局部变量，所以发布方只能在node类里，故Data_Apply不写任何东西，直接在接收下面的回调函数里实现功能。\n",[32,2873,2875,2878,2881,2884,2887,2889],{"class":34,"line":2874},125,[32,2876,2877],{"class":647},"          if",[32,2879,2880],{"class":681},"(serial_pack_.rx.data.cmd ",[32,2882,2883],{"class":647},"==",[32,2885,2886],{"class":647}," 0x",[32,2888,2819],{"class":52},[32,2890,767],{"class":681},[32,2892,2894],{"class":34,"line":2893},126,[32,2895,2770],{"class":681},[32,2897,2899,2902,2904,2906,2908,2910,2912,2914,2917,2919],{"class":34,"line":2898},127,[32,2900,2901],{"class":38},"            RCLCPP_DEBUG",[32,2903,573],{"class":681},[32,2905,1123],{"class":52},[32,2907,1502],{"class":681},[32,2909,1632],{"class":38},[32,2911,1635],{"class":681},[32,2913,1887],{"class":42},[32,2915,2916],{"class":52},"\\n",[32,2918,1887],{"class":42},[32,2920,1519],{"class":681},[32,2922,2924,2926,2928,2930,2932,2934,2936,2939],{"class":34,"line":2923},128,[32,2925,2901],{"class":38},[32,2927,573],{"class":681},[32,2929,1123],{"class":52},[32,2931,1502],{"class":681},[32,2933,1632],{"class":38},[32,2935,1635],{"class":681},[32,2937,2938],{"class":42},"\"以下是电机编码器的odom数据：\"",[32,2940,1519],{"class":681},[32,2942,2944],{"class":34,"line":2943},129,[32,2945,676],{"emptyLinePlaceholder":675},[32,2947,2949,2952],{"class":34,"line":2948},130,[32,2950,2951],{"class":270},"            //",[32,2953,2954],{"class":270}," // 存储电机速度数据\n",[32,2956,2958,2961],{"class":34,"line":2957},131,[32,2959,2960],{"class":270},"            // received_encoder_wheel_velocities_[0] = serial_pack_.rx.data.fp32_buffer[0];",[32,2962,2963],{"class":270},"  // 电机 0 速度\n",[32,2965,2967,2970],{"class":34,"line":2966},132,[32,2968,2969],{"class":270},"            // received_encoder_wheel_velocities_[1] = serial_pack_.rx.data.fp32_buffer[1];",[32,2971,2972],{"class":270},"  // 电机 1 速度\n",[32,2974,2976,2979],{"class":34,"line":2975},133,[32,2977,2978],{"class":270},"            // received_encoder_wheel_velocities_[2] = serial_pack_.rx.data.fp32_buffer[2];",[32,2980,2981],{"class":270},"  // 电机 2 速度\n",[32,2983,2985,2988],{"class":34,"line":2984},134,[32,2986,2987],{"class":270},"            // received_encoder_wheel_velocities_[3] = serial_pack_.rx.data.fp32_buffer[3];",[32,2989,2990],{"class":270},"  // 电机 3 速度\n",[32,2992,2994],{"class":34,"line":2993},135,[32,2995,676],{"emptyLinePlaceholder":675},[32,2997,2999,3001],{"class":34,"line":2998},136,[32,3000,2951],{"class":270},[32,3002,3003],{"class":270}," // 存储电机位置数据\n",[32,3005,3007,3010],{"class":34,"line":3006},137,[32,3008,3009],{"class":270},"            // received_encoder_wheel_angle_[0] = serial_pack_.rx.data.fp32_buffer[4];",[32,3011,3012],{"class":270},"  // 电机 0 位置\n",[32,3014,3016,3019],{"class":34,"line":3015},138,[32,3017,3018],{"class":270},"            // received_encoder_wheel_angle_[1] = serial_pack_.rx.data.fp32_buffer[5];",[32,3020,3021],{"class":270},"  // 电机 1 位置\n",[32,3023,3025,3028],{"class":34,"line":3024},139,[32,3026,3027],{"class":270},"            // received_encoder_wheel_angle_[2] = serial_pack_.rx.data.fp32_buffer[6];",[32,3029,3030],{"class":270},"  // 电机 2 位置\n",[32,3032,3034,3037],{"class":34,"line":3033},140,[32,3035,3036],{"class":270},"            // received_encoder_wheel_angle_[3] = serial_pack_.rx.data.fp32_buffer[7];",[32,3038,3039],{"class":270},"  // 电机 3 位置\n",[32,3041,3043],{"class":34,"line":3042},141,[32,3044,676],{"emptyLinePlaceholder":675},[32,3046,3048],{"class":34,"line":3047},142,[32,3049,3050],{"class":270},"            // vx = serial_pack_.rx.data.fp32_buffer[8];\n",[32,3052,3054],{"class":34,"line":3053},143,[32,3055,3056],{"class":270},"            // vy = serial_pack_.rx.data.fp32_buffer[9];\n",[32,3058,3060],{"class":34,"line":3059},144,[32,3061,3062],{"class":270},"            // vw = serial_pack_.rx.data.fp32_buffer[10];\n",[32,3064,3066],{"class":34,"line":3065},145,[32,3067,3068],{"class":270},"            // yaw_ = serial_pack_.rx.data.fp32_buffer[11];\n",[32,3070,3072],{"class":34,"line":3071},146,[32,3073,3074],{"class":270},"            // dt_ = serial_pack_.rx.data.fp32_buffer[12];\n",[32,3076,3078],{"class":34,"line":3077},147,[32,3079,3080],{"class":270},"            // x_position_ = serial_pack_.rx.data.fp32_buffer[13];\n",[32,3082,3084],{"class":34,"line":3083},148,[32,3085,3086],{"class":270},"            // y_position_ = serial_pack_.rx.data.fp32_buffer[14];\n",[32,3088,3090],{"class":34,"line":3089},149,[32,3091,676],{"emptyLinePlaceholder":675},[32,3093,3095,3098,3100,3103,3105],{"class":34,"line":3094},150,[32,3096,3097],{"class":681},"            vx ",[32,3099,783],{"class":647},[32,3101,3102],{"class":681}," serial_pack_.rx.data.fp32_buffer[",[32,3104,905],{"class":52},[32,3106,3107],{"class":681},"];\n",[32,3109,3111,3114,3116,3118,3120],{"class":34,"line":3110},151,[32,3112,3113],{"class":681},"            vy ",[32,3115,783],{"class":647},[32,3117,3102],{"class":681},[32,3119,917],{"class":52},[32,3121,3107],{"class":681},[32,3123,3125,3128,3130,3132,3134],{"class":34,"line":3124},152,[32,3126,3127],{"class":681},"            vw ",[32,3129,783],{"class":647},[32,3131,3102],{"class":681},[32,3133,927],{"class":52},[32,3135,3107],{"class":681},[32,3137,3139,3142,3144,3146,3148],{"class":34,"line":3138},153,[32,3140,3141],{"class":681},"            yaw_ ",[32,3143,783],{"class":647},[32,3145,3102],{"class":681},[32,3147,936],{"class":52},[32,3149,3107],{"class":681},[32,3151,3153,3156,3158,3160,3163],{"class":34,"line":3152},154,[32,3154,3155],{"class":681},"            dt_ ",[32,3157,783],{"class":647},[32,3159,3102],{"class":681},[32,3161,3162],{"class":52},"4",[32,3164,3107],{"class":681},[32,3166,3168,3171,3173,3175,3178],{"class":34,"line":3167},155,[32,3169,3170],{"class":681},"            x_position_ ",[32,3172,783],{"class":647},[32,3174,3102],{"class":681},[32,3176,3177],{"class":52},"5",[32,3179,3107],{"class":681},[32,3181,3183,3186,3188,3190,3193],{"class":34,"line":3182},156,[32,3184,3185],{"class":681},"            y_position_ ",[32,3187,783],{"class":647},[32,3189,3102],{"class":681},[32,3191,3192],{"class":52},"6",[32,3194,3107],{"class":681},[32,3196,3198],{"class":34,"line":3197},157,[32,3199,676],{"emptyLinePlaceholder":675},[32,3201,3203],{"class":34,"line":3202},158,[32,3204,3205],{"class":270},"            // 打印电机速度和位置（角度）\n",[32,3207,3209,3212,3214,3217,3219,3221,3223,3226,3228,3230,3233,3235],{"class":34,"line":3208},159,[32,3210,3211],{"class":647},"            for",[32,3213,1022],{"class":681},[32,3215,3216],{"class":647},"int",[32,3218,799],{"class":681},[32,3220,783],{"class":647},[32,3222,804],{"class":52},[32,3224,3225],{"class":681},"; i ",[32,3227,810],{"class":647},[32,3229,813],{"class":52},[32,3231,3232],{"class":681},"; ",[32,3234,819],{"class":647},[32,3236,3237],{"class":681},"i) \n",[32,3239,3241],{"class":34,"line":3240},160,[32,3242,3243],{"class":681},"            {\n",[32,3245,3247],{"class":34,"line":3246},161,[32,3248,3249],{"class":270},"                // RCLCPP_DEBUG(this->get_logger(), \"%d号电机的速度: %.6f RPM, 位置: %.6f (2000pc)\",\n",[32,3251,3253],{"class":34,"line":3252},162,[32,3254,3255],{"class":270},"                //             i, received_encoder_wheel_velocities_[i], received_encoder_wheel_angle_[i]);\n",[32,3257,3259],{"class":34,"line":3258},163,[32,3260,676],{"emptyLinePlaceholder":675},[32,3262,3264,3267,3269,3271,3273,3275,3278,3281,3284,3287,3289,3292,3294,3296,3299,3301,3303],{"class":34,"line":3263},164,[32,3265,3266],{"class":38},"                RCLCPP_DEBUG",[32,3268,573],{"class":681},[32,3270,1123],{"class":52},[32,3272,1502],{"class":681},[32,3274,1632],{"class":38},[32,3276,3277],{"class":681},"(),",[32,3279,3280],{"class":42},"\"线速度:x:",[32,3282,3283],{"class":52},"%.6f",[32,3285,3286],{"class":42},",y:",[32,3288,3283],{"class":52},[32,3290,3291],{"class":42},",z:",[32,3293,3283],{"class":52},[32,3295,1887],{"class":42},[32,3297,3298],{"class":681},",vx,vy,",[32,3300,2491],{"class":52},[32,3302,850],{"class":647},[32,3304,1519],{"class":681},[32,3306,3308,3310,3312,3314,3316,3318,3320,3323,3325,3327,3329,3331,3333,3335,3337,3339,3341,3343,3345,3347],{"class":34,"line":3307},165,[32,3309,3266],{"class":38},[32,3311,573],{"class":681},[32,3313,1123],{"class":52},[32,3315,1502],{"class":681},[32,3317,1632],{"class":38},[32,3319,3277],{"class":681},[32,3321,3322],{"class":42},"\"角速度:x:",[32,3324,3283],{"class":52},[32,3326,3286],{"class":42},[32,3328,3283],{"class":52},[32,3330,3291],{"class":42},[32,3332,3283],{"class":52},[32,3334,1887],{"class":42},[32,3336,2060],{"class":681},[32,3338,2491],{"class":52},[32,3340,850],{"class":647},[32,3342,2060],{"class":681},[32,3344,2491],{"class":52},[32,3346,850],{"class":647},[32,3348,3349],{"class":681},",vw);\n",[32,3351,3353,3355,3357,3359,3361,3363,3365,3368,3370,3373,3375,3377,3379,3381,3383,3385,3387,3389,3391,3393],{"class":34,"line":3352},166,[32,3354,3266],{"class":38},[32,3356,573],{"class":681},[32,3358,1123],{"class":52},[32,3360,1502],{"class":681},[32,3362,1632],{"class":38},[32,3364,3277],{"class":681},[32,3366,3367],{"class":42},"\"欧拉角(逆正顺负):r:",[32,3369,3283],{"class":52},[32,3371,3372],{"class":42},",p:",[32,3374,3283],{"class":52},[32,3376,3286],{"class":42},[32,3378,3283],{"class":52},[32,3380,1887],{"class":42},[32,3382,2060],{"class":681},[32,3384,2491],{"class":52},[32,3386,850],{"class":647},[32,3388,2060],{"class":681},[32,3390,2491],{"class":52},[32,3392,850],{"class":647},[32,3394,3395],{"class":681},",yaw_);\n",[32,3397,3399,3401,3403,3405,3407,3409,3411,3414,3416,3418],{"class":34,"line":3398},167,[32,3400,3266],{"class":38},[32,3402,573],{"class":681},[32,3404,1123],{"class":52},[32,3406,1502],{"class":681},[32,3408,1632],{"class":38},[32,3410,3277],{"class":681},[32,3412,3413],{"class":42},"\"积分间隔:",[32,3415,3283],{"class":52},[32,3417,1887],{"class":42},[32,3419,3420],{"class":681},",dt_);\n",[32,3422,3424,3426,3428,3430,3432,3434,3436,3439,3441,3443],{"class":34,"line":3423},168,[32,3425,3266],{"class":38},[32,3427,573],{"class":681},[32,3429,1123],{"class":52},[32,3431,1502],{"class":681},[32,3433,1632],{"class":38},[32,3435,3277],{"class":681},[32,3437,3438],{"class":42},"\"右手坐标系X坐标(前正后负):",[32,3440,3283],{"class":52},[32,3442,1887],{"class":42},[32,3444,3445],{"class":681},",x_position_);\n",[32,3447,3449,3451,3453,3455,3457,3459,3461,3464,3466,3468],{"class":34,"line":3448},169,[32,3450,3266],{"class":38},[32,3452,573],{"class":681},[32,3454,1123],{"class":52},[32,3456,1502],{"class":681},[32,3458,1632],{"class":38},[32,3460,3277],{"class":681},[32,3462,3463],{"class":42},"\"右手坐标系Y坐标(左正右负):",[32,3465,3283],{"class":52},[32,3467,1887],{"class":42},[32,3469,3470],{"class":681},",y_position_);\n",[32,3472,3474],{"class":34,"line":3473},170,[32,3475,3476],{"class":681},"            }\n",[32,3478,3480],{"class":34,"line":3479},171,[32,3481,676],{"emptyLinePlaceholder":675},[32,3483,3485],{"class":34,"line":3484},172,[32,3486,3487],{"class":270},"            //时间戳\n",[32,3489,3491,3494,3496,3498,3500,3503,3505,3508],{"class":34,"line":3490},173,[32,3492,3493],{"class":681},"            odom_msg.header.stamp ",[32,3495,783],{"class":647},[32,3497,1111],{"class":52},[32,3499,1502],{"class":681},[32,3501,3502],{"class":38},"get_clock",[32,3504,1835],{"class":681},[32,3506,3507],{"class":38},"now",[32,3509,1585],{"class":681},[32,3511,3513],{"class":34,"line":3512},174,[32,3514,676],{"emptyLinePlaceholder":675},[32,3516,3518],{"class":34,"line":3517},175,[32,3519,3520],{"class":270},"            //位姿信息所参考的坐标系\n",[32,3522,3524,3527,3529,3532],{"class":34,"line":3523},176,[32,3525,3526],{"class":681},"            odom_msg.header.frame_id ",[32,3528,783],{"class":647},[32,3530,3531],{"class":42}," \"odom\"",[32,3533,874],{"class":681},[32,3535,3537],{"class":34,"line":3536},177,[32,3538,676],{"emptyLinePlaceholder":675},[32,3540,3542],{"class":34,"line":3541},178,[32,3543,3544],{"class":270},"            // 设置child_frame_id（底盘坐标系）\n",[32,3546,3548,3551,3553,3556,3558],{"class":34,"line":3547},179,[32,3549,3550],{"class":681},"            odom_msg.child_frame_id ",[32,3552,783],{"class":647},[32,3554,3555],{"class":42}," \"base_link\"",[32,3557,2436],{"class":681},[32,3559,3560],{"class":270},"  // 设置子坐标系为机器人底盘坐标系\n",[32,3562,3564],{"class":34,"line":3563},180,[32,3565,676],{"emptyLinePlaceholder":675},[32,3567,3569],{"class":34,"line":3568},181,[32,3570,3571],{"class":270},"            //DOF平动位置\n",[32,3573,3575,3578,3580],{"class":34,"line":3574},182,[32,3576,3577],{"class":681},"            odom_msg.pose.pose.position.x ",[32,3579,783],{"class":647},[32,3581,3582],{"class":681}," x_position_;\n",[32,3584,3586,3589,3591],{"class":34,"line":3585},183,[32,3587,3588],{"class":681},"            odom_msg.pose.pose.position.y ",[32,3590,783],{"class":647},[32,3592,3593],{"class":681}," y_position_;\n",[32,3595,3597,3600,3602,3604],{"class":34,"line":3596},184,[32,3598,3599],{"class":681},"            odom_msg.pose.pose.position.z ",[32,3601,783],{"class":647},[32,3603,2518],{"class":52},[32,3605,874],{"class":681},[32,3607,3609],{"class":34,"line":3608},185,[32,3610,676],{"emptyLinePlaceholder":675},[32,3612,3614],{"class":34,"line":3613},186,[32,3615,3616],{"class":270},"            //从欧拉角转换为四元数\n",[32,3618,3620,3623],{"class":34,"line":3619},187,[32,3621,3622],{"class":38},"            tf2",[32,3624,3625],{"class":681},"::Quaternion q;\n",[32,3627,3629,3632,3635,3637,3639,3641,3643],{"class":34,"line":3628},188,[32,3630,3631],{"class":681},"            q.",[32,3633,3634],{"class":38},"setRPY",[32,3636,573],{"class":681},[32,3638,905],{"class":52},[32,3640,2060],{"class":681},[32,3642,905],{"class":52},[32,3644,3395],{"class":681},[32,3646,3648],{"class":34,"line":3647},189,[32,3649,3650],{"class":270},"            //DOF转动（四元数）\n",[32,3652,3654,3657,3659,3662,3665],{"class":34,"line":3653},190,[32,3655,3656],{"class":681},"            odom_msg.pose.pose.orientation.x ",[32,3658,783],{"class":647},[32,3660,3661],{"class":681}," q.",[32,3663,3664],{"class":38},"x",[32,3666,1585],{"class":681},[32,3668,3670,3673,3675,3677,3680],{"class":34,"line":3669},191,[32,3671,3672],{"class":681},"            odom_msg.pose.pose.orientation.y ",[32,3674,783],{"class":647},[32,3676,3661],{"class":681},[32,3678,3679],{"class":38},"y",[32,3681,1585],{"class":681},[32,3683,3685,3688,3690,3692,3695],{"class":34,"line":3684},192,[32,3686,3687],{"class":681},"            odom_msg.pose.pose.orientation.z ",[32,3689,783],{"class":647},[32,3691,3661],{"class":681},[32,3693,3694],{"class":38},"z",[32,3696,1585],{"class":681},[32,3698,3700,3703,3705,3707,3710],{"class":34,"line":3699},193,[32,3701,3702],{"class":681},"            odom_msg.pose.pose.orientation.w ",[32,3704,783],{"class":647},[32,3706,3661],{"class":681},[32,3708,3709],{"class":38},"w",[32,3711,1585],{"class":681},[32,3713,3715],{"class":34,"line":3714},194,[32,3716,676],{"emptyLinePlaceholder":675},[32,3718,3720],{"class":34,"line":3719},195,[32,3721,3722],{"class":270},"            //线速度\n",[32,3724,3726,3729,3731],{"class":34,"line":3725},196,[32,3727,3728],{"class":681},"            odom_msg.twist.twist.linear.x ",[32,3730,783],{"class":647},[32,3732,3733],{"class":681}," vx;\n",[32,3735,3737,3740,3742],{"class":34,"line":3736},197,[32,3738,3739],{"class":681},"            odom_msg.twist.twist.linear.y ",[32,3741,783],{"class":647},[32,3743,3744],{"class":681}," vy;\n",[32,3746,3748,3751,3753,3755],{"class":34,"line":3747},198,[32,3749,3750],{"class":681},"            odom_msg.twist.twist.linear.z ",[32,3752,783],{"class":647},[32,3754,2518],{"class":52},[32,3756,874],{"class":681},[32,3758,3760],{"class":34,"line":3759},199,[32,3761,676],{"emptyLinePlaceholder":675},[32,3763,3765],{"class":34,"line":3764},200,[32,3766,3767],{"class":270},"            //角速度\n",[32,3769,3771,3774,3776,3778],{"class":34,"line":3770},201,[32,3772,3773],{"class":681},"            odom_msg.twist.twist.angular.x ",[32,3775,783],{"class":647},[32,3777,2518],{"class":52},[32,3779,874],{"class":681},[32,3781,3783,3786,3788,3790],{"class":34,"line":3782},202,[32,3784,3785],{"class":681},"            odom_msg.twist.twist.angular.y ",[32,3787,783],{"class":647},[32,3789,2518],{"class":52},[32,3791,874],{"class":681},[32,3793,3795,3798,3800],{"class":34,"line":3794},203,[32,3796,3797],{"class":681},"            odom_msg.twist.twist.angular.z ",[32,3799,783],{"class":647},[32,3801,3802],{"class":681}," vw;\n",[32,3804,3806],{"class":34,"line":3805},204,[32,3807,676],{"emptyLinePlaceholder":675},[32,3809,3811],{"class":34,"line":3810},205,[32,3812,3813],{"class":270},"            // 发布消息\n",[32,3815,3817,3820,3823],{"class":34,"line":3816},206,[32,3818,3819],{"class":681},"            odom_pub_->",[32,3821,3822],{"class":38},"publish",[32,3824,3825],{"class":681},"(odom_msg);\n",[32,3827,3829],{"class":34,"line":3828},207,[32,3830,676],{"emptyLinePlaceholder":675},[32,3832,3834],{"class":34,"line":3833},208,[32,3835,3836],{"class":270},"            // 打印位置（XYZ）\n",[32,3838,3840,3842],{"class":34,"line":3839},209,[32,3841,2901],{"class":38},[32,3843,1669],{"class":681},[32,3845,3847,3850,3852,3854],{"class":34,"line":3846},210,[32,3848,3849],{"class":52},"              this",[32,3851,1502],{"class":681},[32,3853,1632],{"class":38},[32,3855,3856],{"class":681},"(),\n",[32,3858,3860,3863,3865,3867,3869,3871,3873,3875],{"class":34,"line":3859},211,[32,3861,3862],{"class":42},"              \"位置Position(XYZ): ",[32,3864,3283],{"class":52},[32,3866,1513],{"class":42},[32,3868,3283],{"class":52},[32,3870,1513],{"class":42},[32,3872,3283],{"class":52},[32,3874,1887],{"class":42},[32,3876,2822],{"class":681},[32,3878,3880],{"class":34,"line":3879},212,[32,3881,3882],{"class":681},"              odom_msg.pose.pose.position.x,\n",[32,3884,3886],{"class":34,"line":3885},213,[32,3887,3888],{"class":681},"              odom_msg.pose.pose.position.y,\n",[32,3890,3892],{"class":34,"line":3891},214,[32,3893,3894],{"class":681},"              odom_msg.pose.pose.position.z\n",[32,3896,3898],{"class":34,"line":3897},215,[32,3899,3900],{"class":681},"            );\n",[32,3902,3904],{"class":34,"line":3903},216,[32,3905,676],{"emptyLinePlaceholder":675},[32,3907,3909],{"class":34,"line":3908},217,[32,3910,3911],{"class":270},"            // 打印姿态（四元数WXYZ）\n",[32,3913,3915,3917],{"class":34,"line":3914},218,[32,3916,2901],{"class":38},[32,3918,1669],{"class":681},[32,3920,3922,3924,3926,3928],{"class":34,"line":3921},219,[32,3923,3849],{"class":52},[32,3925,1502],{"class":681},[32,3927,1632],{"class":38},[32,3929,3856],{"class":681},[32,3931,3933,3936,3938,3940,3942,3944,3946,3948,3950,3952],{"class":34,"line":3932},220,[32,3934,3935],{"class":42},"              \"姿态Orientation(WXYZ): ",[32,3937,3283],{"class":52},[32,3939,1513],{"class":42},[32,3941,3283],{"class":52},[32,3943,1513],{"class":42},[32,3945,3283],{"class":52},[32,3947,1513],{"class":42},[32,3949,3283],{"class":52},[32,3951,1887],{"class":42},[32,3953,2822],{"class":681},[32,3955,3957,3960],{"class":34,"line":3956},221,[32,3958,3959],{"class":681},"              odom_msg.pose.pose.orientation.w,",[32,3961,3962],{"class":270},"  // 注意顺序：WXYZ\n",[32,3964,3966],{"class":34,"line":3965},222,[32,3967,3968],{"class":681},"              odom_msg.pose.pose.orientation.x,\n",[32,3970,3972],{"class":34,"line":3971},223,[32,3973,3974],{"class":681},"              odom_msg.pose.pose.orientation.y,\n",[32,3976,3978],{"class":34,"line":3977},224,[32,3979,3980],{"class":681},"              odom_msg.pose.pose.orientation.z\n",[32,3982,3984],{"class":34,"line":3983},225,[32,3985,3900],{"class":681},[32,3987,3989],{"class":34,"line":3988},226,[32,3990,676],{"emptyLinePlaceholder":675},[32,3992,3994],{"class":34,"line":3993},227,[32,3995,3996],{"class":270},"            // 打印姿态（欧拉角RPY）\n",[32,3998,4000,4002],{"class":34,"line":3999},228,[32,4001,2901],{"class":38},[32,4003,1669],{"class":681},[32,4005,4007,4009,4011,4013],{"class":34,"line":4006},229,[32,4008,3849],{"class":52},[32,4010,1502],{"class":681},[32,4012,1632],{"class":38},[32,4014,3856],{"class":681},[32,4016,4018,4021,4023,4025,4027,4029,4031,4033],{"class":34,"line":4017},230,[32,4019,4020],{"class":42},"              \"欧拉角Euler(RPY): ",[32,4022,3283],{"class":52},[32,4024,1513],{"class":42},[32,4026,3283],{"class":52},[32,4028,1513],{"class":42},[32,4030,3283],{"class":52},[32,4032,1887],{"class":42},[32,4034,2822],{"class":681},[32,4036,4038,4041],{"class":34,"line":4037},231,[32,4039,4040],{"class":52},"              0.0",[32,4042,2822],{"class":681},[32,4044,4046,4048],{"class":34,"line":4045},232,[32,4047,4040],{"class":52},[32,4049,2822],{"class":681},[32,4051,4053],{"class":34,"line":4052},233,[32,4054,4055],{"class":681},"              yaw_\n",[32,4057,4059],{"class":34,"line":4058},234,[32,4060,3900],{"class":681},[32,4062,4064],{"class":34,"line":4063},235,[32,4065,676],{"emptyLinePlaceholder":675},[32,4067,4069],{"class":34,"line":4068},236,[32,4070,4071],{"class":270},"            // 打印线速度（XYZ）\n",[32,4073,4075,4077],{"class":34,"line":4074},237,[32,4076,2901],{"class":38},[32,4078,1669],{"class":681},[32,4080,4082,4084,4086,4088],{"class":34,"line":4081},238,[32,4083,3849],{"class":52},[32,4085,1502],{"class":681},[32,4087,1632],{"class":38},[32,4089,3856],{"class":681},[32,4091,4093,4096,4098,4100,4102,4104,4106,4108],{"class":34,"line":4092},239,[32,4094,4095],{"class":42},"              \"线速度LinearVel(XYZ): ",[32,4097,3283],{"class":52},[32,4099,1513],{"class":42},[32,4101,3283],{"class":52},[32,4103,1513],{"class":42},[32,4105,3283],{"class":52},[32,4107,1887],{"class":42},[32,4109,2822],{"class":681},[32,4111,4113],{"class":34,"line":4112},240,[32,4114,4115],{"class":681},"              odom_msg.twist.twist.linear.x,\n",[32,4117,4119],{"class":34,"line":4118},241,[32,4120,4121],{"class":681},"              odom_msg.twist.twist.linear.y,\n",[32,4123,4125],{"class":34,"line":4124},242,[32,4126,4127],{"class":681},"              odom_msg.twist.twist.linear.z\n",[32,4129,4131],{"class":34,"line":4130},243,[32,4132,3900],{"class":681},[32,4134,4136],{"class":34,"line":4135},244,[32,4137,676],{"emptyLinePlaceholder":675},[32,4139,4141],{"class":34,"line":4140},245,[32,4142,4143],{"class":270},"            // 打印角速度（XYZ）\n",[32,4145,4147,4149],{"class":34,"line":4146},246,[32,4148,2901],{"class":38},[32,4150,1669],{"class":681},[32,4152,4154,4156,4158,4160],{"class":34,"line":4153},247,[32,4155,3849],{"class":52},[32,4157,1502],{"class":681},[32,4159,1632],{"class":38},[32,4161,3856],{"class":681},[32,4163,4165,4168,4170,4172,4174,4176,4178,4180],{"class":34,"line":4164},248,[32,4166,4167],{"class":42},"              \"角速度AngularVel(XYZ): ",[32,4169,3283],{"class":52},[32,4171,1513],{"class":42},[32,4173,3283],{"class":52},[32,4175,1513],{"class":42},[32,4177,3283],{"class":52},[32,4179,1887],{"class":42},[32,4181,2822],{"class":681},[32,4183,4185],{"class":34,"line":4184},249,[32,4186,4187],{"class":681},"              odom_msg.twist.twist.angular.x,\n",[32,4189,4191],{"class":34,"line":4190},250,[32,4192,4193],{"class":681},"              odom_msg.twist.twist.angular.y,\n",[32,4195,4197],{"class":34,"line":4196},251,[32,4198,4199],{"class":681},"              odom_msg.twist.twist.angular.z\n",[32,4201,4203],{"class":34,"line":4202},252,[32,4204,3900],{"class":681},[32,4206,4208],{"class":34,"line":4207},253,[32,4209,676],{"emptyLinePlaceholder":675},[32,4211,4213],{"class":34,"line":4212},254,[32,4214,4215],{"class":270},"            /* 接下来发布tf */\n",[32,4217,4219,4222,4225,4227,4229,4231,4233,4235,4238],{"class":34,"line":4218},255,[32,4220,4221],{"class":647},"            auto",[32,4223,4224],{"class":681}," transform ",[32,4226,783],{"class":647},[32,4228,68],{"class":38},[32,4230,752],{"class":681},[32,4232,2047],{"class":38},[32,4234,752],{"class":681},[32,4236,4237],{"class":38},"TransformStamped",[32,4239,1585],{"class":681},[32,4241,4243,4246,4248,4251],{"class":34,"line":4242},256,[32,4244,4245],{"class":681},"            transform.header.stamp ",[32,4247,783],{"class":647},[32,4249,4250],{"class":681}," odom_msg.header.stamp;",[32,4252,4253],{"class":270}," // 时间戳与odom同步\n",[32,4255,4257,4260,4262,4264],{"class":34,"line":4256},257,[32,4258,4259],{"class":681},"            transform.header.frame_id ",[32,4261,783],{"class":647},[32,4263,3531],{"class":42},[32,4265,874],{"class":681},[32,4267,4269,4272,4274,4276],{"class":34,"line":4268},258,[32,4270,4271],{"class":681},"            transform.child_frame_id ",[32,4273,783],{"class":647},[32,4275,3555],{"class":42},[32,4277,874],{"class":681},[32,4279,4281,4284,4286],{"class":34,"line":4280},259,[32,4282,4283],{"class":681},"            transform.transform.translation.x ",[32,4285,783],{"class":647},[32,4287,3582],{"class":681},[32,4289,4291,4294,4296],{"class":34,"line":4290},260,[32,4292,4293],{"class":681},"            transform.transform.translation.y ",[32,4295,783],{"class":647},[32,4297,3593],{"class":681},[32,4299,4301,4304,4306,4308],{"class":34,"line":4300},261,[32,4302,4303],{"class":681},"            transform.transform.translation.z ",[32,4305,783],{"class":647},[32,4307,2518],{"class":52},[32,4309,874],{"class":681},[32,4311,4313,4316,4318,4321],{"class":34,"line":4312},262,[32,4314,4315],{"class":681},"            transform.transform.rotation ",[32,4317,783],{"class":647},[32,4319,4320],{"class":681}," odom_msg.pose.pose.orientation;",[32,4322,4323],{"class":270}," // 复用四元数\n",[32,4325,4327,4330,4333],{"class":34,"line":4326},263,[32,4328,4329],{"class":681},"            tf_broadcaster_->",[32,4331,4332],{"class":38},"sendTransform",[32,4334,4335],{"class":681},"(transform);\n",[32,4337,4339],{"class":34,"line":4338},264,[32,4340,2865],{"class":681},[32,4342,4344],{"class":34,"line":4343},265,[32,4345,676],{"emptyLinePlaceholder":675},[32,4347,4349],{"class":34,"line":4348},266,[32,4350,4351],{"class":681},"        }\n",[32,4353,4355],{"class":34,"line":4354},267,[32,4356,4357],{"class":270},"        // 继续监听新的数据\n",[32,4359,4361,4364],{"class":34,"line":4360},268,[32,4362,4363],{"class":38},"        async_receive_message",[32,4365,1585],{"class":681},[32,4367,4369],{"class":34,"line":4368},269,[32,4370,879],{"class":681},[32,4372,4374],{"class":34,"line":4373},270,[32,4375,4376],{"class":681},"    );\n",[32,4378,4380],{"class":34,"line":4379},271,[32,4381,2213],{"class":681},[32,4383,4385],{"class":34,"line":4384},272,[32,4386,676],{"emptyLinePlaceholder":675},[32,4388,4390,4392,4395,4397,4399,4401,4403,4405,4407,4409,4411,4414],{"class":34,"line":4389},273,[32,4391,2575],{"class":647},[32,4393,4394],{"class":38}," cmd_vel_sub_callback",[32,4396,573],{"class":681},[32,4398,1955],{"class":647},[32,4400,68],{"class":38},[32,4402,752],{"class":681},[32,4404,2047],{"class":38},[32,4406,752],{"class":681},[32,4408,2352],{"class":38},[32,4410,2667],{"class":647},[32,4412,4413],{"class":763},"cmd_msg",[32,4415,767],{"class":681},[32,4417,4419],{"class":34,"line":4418},274,[32,4420,1485],{"class":681},[32,4422,4424],{"class":34,"line":4423},275,[32,4425,4426],{"class":270},"    // bool bool_buffer[] = {1, 0, 1, 0};\n",[32,4428,4430],{"class":34,"line":4429},276,[32,4431,4432],{"class":270},"    // int8_t int8_buffer[] = {0x11,0x22};\n",[32,4434,4436],{"class":34,"line":4435},277,[32,4437,4438],{"class":270},"    // int16_t int16_buffer[] = {2000,6666};\n",[32,4440,4442],{"class":34,"line":4441},278,[32,4443,4444],{"class":270},"    // int32_t int32_buffer[] = {305419896};\n",[32,4446,4448,4451,4453],{"class":34,"line":4447},279,[32,4449,4450],{"class":681},"    fp32 fp32_buffer[] ",[32,4452,783],{"class":647},[32,4454,4455],{"class":681}," {(fp32)cmd_msg.linear.x,(fp32)cmd_msg.linear.y,(fp32)cmd_msg.linear.z,\n",[32,4457,4459],{"class":34,"line":4458},280,[32,4460,4461],{"class":681},"                          (fp32)cmd_msg.angular.x,(fp32)cmd_msg.angular.y,(fp32)cmd_msg.angular.z};\n",[32,4463,4465],{"class":34,"line":4464},281,[32,4466,676],{"emptyLinePlaceholder":675},[32,4468,4470],{"class":34,"line":4469},282,[32,4471,4472],{"class":270},"    //由于ROS2中node为局部变量，所以只能在node中调用send函数，所以Serial_Transmit只负责处理data_buffer。\n",[32,4474,4476,4479,4482,4484,4487,4489],{"class":34,"line":4475},283,[32,4477,4478],{"class":681},"    serial_pack_.tx.",[32,4480,4481],{"class":38},"Data_Pack",[32,4483,573],{"class":681},[32,4485,4486],{"class":647},"0x",[32,4488,2819],{"class":52},[32,4490,4491],{"class":681},", \n",[32,4493,4495,4498,4500,4502],{"class":34,"line":4494},284,[32,4496,4497],{"class":52},"                                 nullptr",[32,4499,1513],{"class":681},[32,4501,905],{"class":52},[32,4503,2822],{"class":681},[32,4505,4507,4509,4511,4513],{"class":34,"line":4506},285,[32,4508,4497],{"class":52},[32,4510,1513],{"class":681},[32,4512,905],{"class":52},[32,4514,2822],{"class":681},[32,4516,4518,4520,4522,4524],{"class":34,"line":4517},286,[32,4519,4497],{"class":52},[32,4521,1513],{"class":681},[32,4523,905],{"class":52},[32,4525,2822],{"class":681},[32,4527,4529,4531,4533,4535],{"class":34,"line":4528},287,[32,4530,4497],{"class":52},[32,4532,1513],{"class":681},[32,4534,905],{"class":52},[32,4536,2822],{"class":681},[32,4538,4540,4543,4546,4549,4551,4554],{"class":34,"line":4539},288,[32,4541,4542],{"class":681},"                                 fp32_buffer, ",[32,4544,4545],{"class":647},"sizeof",[32,4547,4548],{"class":681},"(fp32_buffer) ",[32,4550,866],{"class":647},[32,4552,4553],{"class":647}," sizeof",[32,4555,4556],{"class":681},"(fp32));\n",[32,4558,4560],{"class":34,"line":4559},289,[32,4561,676],{"emptyLinePlaceholder":675},[32,4563,4565,4567,4569,4571,4573,4575],{"class":34,"line":4564},290,[32,4566,2595],{"class":647},[32,4568,2598],{"class":681},[32,4570,783],{"class":647},[32,4572,2603],{"class":681},[32,4574,1832],{"class":38},[32,4576,1585],{"class":681},[32,4578,4580],{"class":34,"line":4579},291,[32,4581,676],{"emptyLinePlaceholder":675},[32,4583,4585],{"class":34,"line":4584},292,[32,4586,1736],{"class":647},[32,4588,4590],{"class":34,"line":4589},293,[32,4591,826],{"class":681},[32,4593,4595,4598,4601,4603,4606,4609],{"class":34,"line":4594},294,[32,4596,4597],{"class":647},"      size_t",[32,4599,4600],{"class":681}," bytes_size ",[32,4602,783],{"class":647},[32,4604,4605],{"class":681}," port->",[32,4607,4608],{"class":38},"send",[32,4610,4611],{"class":681},"(tx_data_buffer);\n",[32,4613,4615],{"class":34,"line":4614},295,[32,4616,676],{"emptyLinePlaceholder":675},[32,4618,4620,4623,4625,4627,4629,4631,4633,4636,4638,4640,4642,4644,4646,4648,4651,4653,4656,4658,4660,4662],{"class":34,"line":4619},296,[32,4621,4622],{"class":38},"      RCLCPP_DEBUG",[32,4624,573],{"class":681},[32,4626,1123],{"class":52},[32,4628,1502],{"class":681},[32,4630,1632],{"class":38},[32,4632,1635],{"class":681},[32,4634,4635],{"class":42},"\"平动XYZ：",[32,4637,3283],{"class":52},[32,4639,2060],{"class":42},[32,4641,3283],{"class":52},[32,4643,2060],{"class":42},[32,4645,3283],{"class":52},[32,4647,1887],{"class":42},[32,4649,4650],{"class":681},", fp32_buffer[",[32,4652,905],{"class":52},[32,4654,4655],{"class":681},"],fp32_buffer[",[32,4657,917],{"class":52},[32,4659,4655],{"class":681},[32,4661,927],{"class":52},[32,4663,4664],{"class":681},"]);\n",[32,4666,4668,4670,4672,4674,4676,4678,4680,4683,4685,4687,4689,4691,4693,4695,4697,4699,4701,4703,4705,4707],{"class":34,"line":4667},297,[32,4669,4622],{"class":38},[32,4671,573],{"class":681},[32,4673,1123],{"class":52},[32,4675,1502],{"class":681},[32,4677,1632],{"class":38},[32,4679,1635],{"class":681},[32,4681,4682],{"class":42},"\"转动XYZ：",[32,4684,3283],{"class":52},[32,4686,2060],{"class":42},[32,4688,3283],{"class":52},[32,4690,2060],{"class":42},[32,4692,3283],{"class":52},[32,4694,1887],{"class":42},[32,4696,4650],{"class":681},[32,4698,936],{"class":52},[32,4700,4655],{"class":681},[32,4702,3162],{"class":52},[32,4704,4655],{"class":681},[32,4706,3177],{"class":52},[32,4708,4664],{"class":681},[32,4710,4712,4714,4716,4718,4720,4722,4724,4727,4730,4733],{"class":34,"line":4711},298,[32,4713,4622],{"class":38},[32,4715,573],{"class":681},[32,4717,1123],{"class":52},[32,4719,1502],{"class":681},[32,4721,1632],{"class":38},[32,4723,1635],{"class":681},[32,4725,4726],{"class":42},"\"(",[32,4728,4729],{"class":52},"%ld",[32,4731,4732],{"class":42}," bytes)\"",[32,4734,4735],{"class":681},", bytes_size);\n",[32,4737,4739],{"class":34,"line":4738},299,[32,4740,879],{"class":681},[32,4742,4744,4746,4748,4750,4752,4754,4756],{"class":34,"line":4743},300,[32,4745,1950],{"class":647},[32,4747,573],{"class":681},[32,4749,1955],{"class":647},[32,4751,1419],{"class":38},[32,4753,1960],{"class":681},[32,4755,1963],{"class":647},[32,4757,1966],{"class":681},[32,4759,4761],{"class":34,"line":4760},301,[32,4762,826],{"class":681},[32,4764,4766,4768,4770,4772,4774,4776,4778,4781,4783,4785,4788,4790],{"class":34,"line":4765},302,[32,4767,1975],{"class":38},[32,4769,573],{"class":681},[32,4771,1123],{"class":52},[32,4773,1502],{"class":681},[32,4775,1632],{"class":38},[32,4777,1635],{"class":681},[32,4779,4780],{"class":42},"\"Error Transmiting from serial port:",[32,4782,1884],{"class":52},[32,4784,1887],{"class":42},[32,4786,4787],{"class":681},",ex.",[32,4789,1998],{"class":38},[32,4791,1911],{"class":681},[32,4793,4795],{"class":34,"line":4794},303,[32,4796,879],{"class":681},[32,4798,4800],{"class":34,"line":4799},304,[32,4801,2213],{"class":681},[32,4803,4805],{"class":34,"line":4804},305,[32,4806,4807],{"class":681},"};\n",[32,4809,4811],{"class":34,"line":4810},306,[32,4812,676],{"emptyLinePlaceholder":675},[32,4814,4816,4818,4821,4823,4825,4828,4830,4833,4836,4839],{"class":34,"line":4815},307,[32,4817,3216],{"class":647},[32,4819,4820],{"class":38}," main",[32,4822,573],{"class":681},[32,4824,3216],{"class":647},[32,4826,4827],{"class":763}," argc",[32,4829,1513],{"class":681},[32,4831,4832],{"class":647},"char",[32,4834,4835],{"class":647}," **",[32,4837,4838],{"class":763},"argv",[32,4840,767],{"class":681},[32,4842,4844],{"class":34,"line":4843},308,[32,4845,772],{"class":681},[32,4847,4849,4851,4853,4856],{"class":34,"line":4848},309,[32,4850,2283],{"class":38},[32,4852,752],{"class":681},[32,4854,4855],{"class":38},"init",[32,4857,4858],{"class":681},"(argc, argv);\n",[32,4860,4862],{"class":34,"line":4861},310,[32,4863,676],{"emptyLinePlaceholder":675},[32,4865,4867,4870,4873,4875,4877,4879,4881,4883,4885],{"class":34,"line":4866},311,[32,4868,4869],{"class":647},"  auto",[32,4871,4872],{"class":681}," node ",[32,4874,783],{"class":647},[32,4876,1419],{"class":38},[32,4878,752],{"class":681},[32,4880,1754],{"class":38},[32,4882,810],{"class":681},[32,4884,2173],{"class":38},[32,4886,4887],{"class":681},">();\n",[32,4889,4891],{"class":34,"line":4890},312,[32,4892,676],{"emptyLinePlaceholder":675},[32,4894,4896,4898,4900,4903],{"class":34,"line":4895},313,[32,4897,2283],{"class":38},[32,4899,752],{"class":681},[32,4901,4902],{"class":38},"spin",[32,4904,4905],{"class":681},"(node);\n",[32,4907,4909],{"class":34,"line":4908},314,[32,4910,676],{"emptyLinePlaceholder":675},[32,4912,4914,4916,4918,4921],{"class":34,"line":4913},315,[32,4915,2283],{"class":38},[32,4917,752],{"class":681},[32,4919,4920],{"class":38},"shutdown",[32,4922,1585],{"class":681},[32,4924,4926,4929,4931],{"class":34,"line":4925},316,[32,4927,4928],{"class":647},"  return",[32,4930,804],{"class":52},[32,4932,874],{"class":681},[32,4934,4936],{"class":34,"line":4935},317,[32,4937,1323],{"class":681},[15,4939,4940],{},[599,4941],{"alt":27,"src":4942},"https://cdn.tungchiahui.cn/tungwebsite/assets/images/2023/12/30/image1799.webp",[15,4944,4945],{},"The final odom effective data should be around 250Hz, published every 4ms, which is fully sufficient for navigation.",[4947,4948,4949],"style",{},"html pre.shiki code .sScJk, html code.shiki .sScJk{--shiki-default:#6F42C1;--shiki-dark:#B392F0}html pre.shiki code .sZZnC, html code.shiki .sZZnC{--shiki-default:#032F62;--shiki-dark:#9ECBFF}html pre.shiki code .sj4cs, html code.shiki .sj4cs{--shiki-default:#005CC5;--shiki-dark:#79B8FF}html .default .shiki span {color: var(--shiki-default);background: var(--shiki-default-bg);font-style: var(--shiki-default-font-style);font-weight: var(--shiki-default-font-weight);text-decoration: var(--shiki-default-text-decoration);}html .shiki span {color: var(--shiki-default);background: var(--shiki-default-bg);font-style: var(--shiki-default-font-style);font-weight: var(--shiki-default-font-weight);text-decoration: var(--shiki-default-text-decoration);}html .dark .shiki span {color: var(--shiki-dark);background: var(--shiki-dark-bg);font-style: var(--shiki-dark-font-style);font-weight: var(--shiki-dark-font-weight);text-decoration: var(--shiki-dark-text-decoration);}html.dark .shiki span {color: var(--shiki-dark);background: var(--shiki-dark-bg);font-style: var(--shiki-dark-font-style);font-weight: var(--shiki-dark-font-weight);text-decoration: var(--shiki-dark-text-decoration);}html pre.shiki code .sJ8bj, html code.shiki .sJ8bj{--shiki-default:#6A737D;--shiki-dark:#6A737D}html pre.shiki code .szBVR, html code.shiki .szBVR{--shiki-default:#D73A49;--shiki-dark:#F97583}html pre.shiki code .sVt8B, html code.shiki .sVt8B{--shiki-default:#24292E;--shiki-dark:#E1E4E8}html pre.shiki code .s4XuR, html code.shiki .s4XuR{--shiki-default:#E36209;--shiki-dark:#FFAB70}",{"title":27,"searchDepth":274,"depth":274,"links":4951},[4952,4953,4954],{"id":12,"depth":286,"text":13},{"id":102,"depth":286,"text":103},{"id":587,"depth":286,"text":588},"/en-us/wiki/2023-12-30-ros2-tutorial/ch14-6-1-li-cheng-ji-odom","14.6.1",14060100,"2023-12-30","wiki/2023-12-30-ros2-tutorial","en-us:2023-12-30-ros2-tutorial","/en-us/wiki/2023-12-30-ros2-tutorial","Ros2 Tutorial","md","wiki/2023-12-30-ros2-tutorial/ch14-6-1-里程计Odom",false,null,"en-US","en-us",{},{"title":5,"description":27},"/wiki/2023-12-30-ros2-tutorial/ch14-6-1-li-cheng-ji-odom","_i18n/en-us/wiki/2023-12-30-ros2-tutorial/ch14-6-1-里程计Odom","qeIG91LyIXTVwOHEaJUCEaTNYGKio0Irg21J_8jsWUo",[4975,4980,4985,4991,4997,5003,5009,5015,5021,5027,5033,5039,5045,5051,5057,5063,5069,5075,5080,5086,5091,5097,5103,5109,5115,5121,5127,5133,5139,5145,5146,5152,5158,5164,5170,5176,5182,5188,5194,5200,5206,5212,5218,5224,5230,5236,5242,5248,5254,5260,5266,5271,5277,5282,5287,5292,5297,5303,5309,5315],{"path":4976,"stem":4977,"title":4978,"date":4958,"chapter":917,"chapterSort":4979,"docKey":4960,"docRoot":4961,"docTitle":4962,"isWikiDoc":675,"isWikiIndex":4965},"/en-us/wiki/2023-12-30-ros2-tutorial/ch1-ros2-jie-shao","_i18n/en-us/wiki/2023-12-30-ros2-tutorial/ch1-ROS2介绍","Introduction to ROS2",1000000,{"path":4981,"stem":4982,"title":4983,"date":4958,"chapter":2063,"chapterSort":4984,"docKey":4960,"docRoot":4961,"docTitle":4962,"isWikiDoc":675,"isWikiIndex":4965},"/en-us/wiki/2023-12-30-ros2-tutorial/ch10-stage-ros2-fang-zhen-ping-tai","_i18n/en-us/wiki/2023-12-30-ros2-tutorial/ch10-Stage_Ros2仿真平台","Stage_Ros2 Simulation Platform",10000000,{"path":4986,"stem":4987,"title":4988,"date":4958,"chapter":4989,"chapterSort":4990,"docKey":4960,"docRoot":4961,"docTitle":4962,"isWikiDoc":675,"isWikiIndex":4965},"/en-us/wiki/2023-12-30-ros2-tutorial/ch11-1-gazeboclassic","_i18n/en-us/wiki/2023-12-30-ros2-tutorial/ch11-1-GazeboClassic","Gezebo Classic","11.1",11010000,{"path":4992,"stem":4993,"title":4994,"date":4958,"chapter":4995,"chapterSort":4996,"docKey":4960,"docRoot":4961,"docTitle":4962,"isWikiDoc":675,"isWikiIndex":4965},"/en-us/wiki/2023-12-30-ros2-tutorial/ch11-2-ignitiongazebo","_i18n/en-us/wiki/2023-12-30-ros2-tutorial/ch11-2-IgnitionGazebo","Ignition Gazebo（Gazebo Fortress）","11.2",11020000,{"path":4998,"stem":4999,"title":5000,"date":4958,"chapter":5001,"chapterSort":5002,"docKey":4960,"docRoot":4961,"docTitle":4962,"isWikiDoc":675,"isWikiIndex":4965},"/en-us/wiki/2023-12-30-ros2-tutorial/ch11-3-gazebosim","_i18n/en-us/wiki/2023-12-30-ros2-tutorial/ch11-3-GazeboSim","Gz Sim（Gazebo Harmonic）","11.3",11030000,{"path":5004,"stem":5005,"title":5006,"date":4958,"chapter":5007,"chapterSort":5008,"docKey":4960,"docRoot":4961,"docTitle":4962,"isWikiDoc":675,"isWikiIndex":4965},"/en-us/wiki/2023-12-30-ros2-tutorial/ch11-4-igngz2gzsim","_i18n/en-us/wiki/2023-12-30-ros2-tutorial/ch11-4-Igngz2gzsim","Migrate Ign Gazebo to Gz Sim","11.4",11040000,{"path":5010,"stem":5011,"title":5012,"date":4958,"chapter":5013,"chapterSort":5014,"docKey":4960,"docRoot":4961,"docTitle":4962,"isWikiDoc":675,"isWikiIndex":4965},"/en-us/wiki/2023-12-30-ros2-tutorial/ch11-gezebo-fang-zhen-ping-tai","_i18n/en-us/wiki/2023-12-30-ros2-tutorial/ch11-Gezebo仿真平台","Gazebo simulation platform","11",11000000,{"path":5016,"stem":5017,"title":5018,"date":4958,"chapter":5019,"chapterSort":5020,"docKey":4960,"docRoot":4961,"docTitle":4962,"isWikiDoc":675,"isWikiIndex":4965},"/en-us/wiki/2023-12-30-ros2-tutorial/ch12-1-humble-ban-ben","_i18n/en-us/wiki/2023-12-30-ros2-tutorial/ch12-1-Humble版本","Humble Navigation Simulation","12.1",12010000,{"path":5022,"stem":5023,"title":5024,"date":4958,"chapter":5025,"chapterSort":5026,"docKey":4960,"docRoot":4961,"docTitle":4962,"isWikiDoc":675,"isWikiIndex":4965},"/en-us/wiki/2023-12-30-ros2-tutorial/ch12-2-jazzy-ban-ben","_i18n/en-us/wiki/2023-12-30-ros2-tutorial/ch12-2-Jazzy版本","Jazzy Navigation Simulation","12.2",12020000,{"path":5028,"stem":5029,"title":5030,"date":4958,"chapter":5031,"chapterSort":5032,"docKey":4960,"docRoot":4961,"docTitle":4962,"isWikiDoc":675,"isWikiIndex":4965},"/en-us/wiki/2023-12-30-ros2-tutorial/ch12-3-humble2jazzy","_i18n/en-us/wiki/2023-12-30-ros2-tutorial/ch12-3-Humble2Jazzy","Differences in Navigation between Humble and Jazzy","12.3",12030000,{"path":5034,"stem":5035,"title":5036,"date":4958,"chapter":5037,"chapterSort":5038,"docKey":4960,"docRoot":4961,"docTitle":4962,"isWikiDoc":675,"isWikiIndex":4965},"/en-us/wiki/2023-12-30-ros2-tutorial/ch12-ji-qi-ren-dao-hang-navigation2-fang-zhen-pian","_i18n/en-us/wiki/2023-12-30-ros2-tutorial/ch12-机器人导航Navigation2(仿真篇)","Robot Navigation with Navigation2 (Simulation)","12",12000000,{"path":5040,"stem":5041,"title":5042,"date":4958,"chapter":5043,"chapterSort":5044,"docKey":4960,"docRoot":4961,"docTitle":4962,"isWikiDoc":675,"isWikiIndex":4965},"/en-us/wiki/2023-12-30-ros2-tutorial/ch13-1-1-boost-aiso","_i18n/en-us/wiki/2023-12-30-ros2-tutorial/ch13-1-1-Boost.Aiso","Boost.Asio库","13.1.1",13010100,{"path":5046,"stem":5047,"title":5048,"date":4958,"chapter":5049,"chapterSort":5050,"docKey":4960,"docRoot":4961,"docTitle":4962,"isWikiDoc":675,"isWikiIndex":4965},"/en-us/wiki/2023-12-30-ros2-tutorial/ch13-1-2-ros2-serial-driver","_i18n/en-us/wiki/2023-12-30-ros2-tutorial/ch13-1-2-ros2_serial_driver","ROS2 Serial Driver Library","13.1.2",13010200,{"path":5052,"stem":5053,"title":5054,"date":4958,"chapter":5055,"chapterSort":5056,"docKey":4960,"docRoot":4961,"docTitle":4962,"isWikiDoc":675,"isWikiIndex":4965},"/en-us/wiki/2023-12-30-ros2-tutorial/ch13-1-chuan-kou-tong-xin","_i18n/en-us/wiki/2023-12-30-ros2-tutorial/ch13-1-串口通信","UART serial communication","13.1",13010000,{"path":5058,"stem":5059,"title":5060,"date":4958,"chapter":5061,"chapterSort":5062,"docKey":4960,"docRoot":4961,"docTitle":4962,"isWikiDoc":675,"isWikiIndex":4965},"/en-us/wiki/2023-12-30-ros2-tutorial/ch13-2-1-socketcan","_i18n/en-us/wiki/2023-12-30-ros2-tutorial/ch13-2-1-socketcan","SocketCAN","13.2.1",13020100,{"path":5064,"stem":5065,"title":5066,"date":4958,"chapter":5067,"chapterSort":5068,"docKey":4960,"docRoot":4961,"docTitle":4962,"isWikiDoc":675,"isWikiIndex":4965},"/en-us/wiki/2023-12-30-ros2-tutorial/ch13-2-2-ros2-socketcan","_i18n/en-us/wiki/2023-12-30-ros2-tutorial/ch13-2-2-ros2_socketcan","ROS2_SocketCAN","13.2.2",13020200,{"path":5070,"stem":5071,"title":5072,"date":4958,"chapter":5073,"chapterSort":5074,"docKey":4960,"docRoot":4961,"docTitle":4962,"isWikiDoc":675,"isWikiIndex":4965},"/en-us/wiki/2023-12-30-ros2-tutorial/ch13-2-can-tong-xin","_i18n/en-us/wiki/2023-12-30-ros2-tutorial/ch13-2-CAN通信","CAN communication","13.2",13020000,{"path":5076,"stem":5077,"title":5042,"date":4958,"chapter":5078,"chapterSort":5079,"docKey":4960,"docRoot":4961,"docTitle":4962,"isWikiDoc":675,"isWikiIndex":4965},"/en-us/wiki/2023-12-30-ros2-tutorial/ch13-3-1-boost-aiso","_i18n/en-us/wiki/2023-12-30-ros2-tutorial/ch13-3-1-Boost.Aiso","13.3.1",13030100,{"path":5081,"stem":5082,"title":5083,"date":4958,"chapter":5084,"chapterSort":5085,"docKey":4960,"docRoot":4961,"docTitle":4962,"isWikiDoc":675,"isWikiIndex":4965},"/en-us/wiki/2023-12-30-ros2-tutorial/ch13-3-tcp-tong-xin","_i18n/en-us/wiki/2023-12-30-ros2-tutorial/ch13-3-TCP通信","TCP communication","13.3",13030000,{"path":5087,"stem":5088,"title":5042,"date":4958,"chapter":5089,"chapterSort":5090,"docKey":4960,"docRoot":4961,"docTitle":4962,"isWikiDoc":675,"isWikiIndex":4965},"/en-us/wiki/2023-12-30-ros2-tutorial/ch13-4-1-boost-aiso","_i18n/en-us/wiki/2023-12-30-ros2-tutorial/ch13-4-1-Boost.Aiso","13.4.1",13040100,{"path":5092,"stem":5093,"title":5094,"date":4958,"chapter":5095,"chapterSort":5096,"docKey":4960,"docRoot":4961,"docTitle":4962,"isWikiDoc":675,"isWikiIndex":4965},"/en-us/wiki/2023-12-30-ros2-tutorial/ch13-4-udp-tong-xin","_i18n/en-us/wiki/2023-12-30-ros2-tutorial/ch13-4-UDP通信","UDP communication","13.4",13040000,{"path":5098,"stem":5099,"title":5100,"date":4958,"chapter":5101,"chapterSort":5102,"docKey":4960,"docRoot":4961,"docTitle":4962,"isWikiDoc":675,"isWikiIndex":4965},"/en-us/wiki/2023-12-30-ros2-tutorial/ch13-linux-ying-jian-tong-xin","_i18n/en-us/wiki/2023-12-30-ros2-tutorial/ch13-Linux硬件通信","Linux hardware communication","13",13000000,{"path":5104,"stem":5105,"title":5106,"date":4958,"chapter":5107,"chapterSort":5108,"docKey":4960,"docRoot":4961,"docTitle":4962,"isWikiDoc":675,"isWikiIndex":4965},"/en-us/wiki/2023-12-30-ros2-tutorial/ch14-1-ji-qi-ren-zu-cheng","_i18n/en-us/wiki/2023-12-30-ros2-tutorial/ch14-1-机器人组成","Robot Composition","14.1",14010000,{"path":5110,"stem":5111,"title":5112,"date":4958,"chapter":5113,"chapterSort":5114,"docKey":4960,"docRoot":4961,"docTitle":4962,"isWikiDoc":675,"isWikiIndex":4965},"/en-us/wiki/2023-12-30-ros2-tutorial/ch14-2-1-yuan-cheng-kai-fa-ssh","_i18n/en-us/wiki/2023-12-30-ros2-tutorial/ch14-2-1-远程开发SSH","Remote Development SSH","14.2.1",14020100,{"path":5116,"stem":5117,"title":5118,"date":4958,"chapter":5119,"chapterSort":5120,"docKey":4960,"docRoot":4961,"docTitle":4962,"isWikiDoc":675,"isWikiIndex":4965},"/en-us/wiki/2023-12-30-ros2-tutorial/ch14-2-2-yuan-cheng-fang-wen-zhuo-mian","_i18n/en-us/wiki/2023-12-30-ros2-tutorial/ch14-2-2-远程访问桌面","Remote Desktop Access","14.2.2",14020200,{"path":5122,"stem":5123,"title":5124,"date":4958,"chapter":5125,"chapterSort":5126,"docKey":4960,"docRoot":4961,"docTitle":4962,"isWikiDoc":675,"isWikiIndex":4965},"/en-us/wiki/2023-12-30-ros2-tutorial/ch14-2-gong-kong-ji-zhi-yuan-cheng-kai-fa-huan-jing","_i18n/en-us/wiki/2023-12-30-ros2-tutorial/ch14-2-工控机之远程开发环境","Remote Development Environment for Industrial PC","14.2",14020000,{"path":5128,"stem":5129,"title":5130,"date":4958,"chapter":5131,"chapterSort":5132,"docKey":4960,"docRoot":4961,"docTitle":4962,"isWikiDoc":675,"isWikiIndex":4965},"/en-us/wiki/2023-12-30-ros2-tutorial/ch14-3-gong-kong-ji-zhi-wai-jie-usb-she-bei","_i18n/en-us/wiki/2023-12-30-ros2-tutorial/ch14-3-工控机之外接USB设备","External USB Devices for the Industrial PC","14.3",14030000,{"path":5134,"stem":5135,"title":5136,"date":4958,"chapter":5137,"chapterSort":5138,"docKey":4960,"docRoot":4961,"docTitle":4962,"isWikiDoc":675,"isWikiIndex":4965},"/en-us/wiki/2023-12-30-ros2-tutorial/ch14-4-fen-bu-shi-da-jian","_i18n/en-us/wiki/2023-12-30-ros2-tutorial/ch14-4-分布式搭建","Distributed setup","14.4",14040000,{"path":5140,"stem":5141,"title":5142,"date":4958,"chapter":5143,"chapterSort":5144,"docKey":4960,"docRoot":4961,"docTitle":4962,"isWikiDoc":675,"isWikiIndex":4965},"/en-us/wiki/2023-12-30-ros2-tutorial/ch14-5-you-hua-ri-zhi","_i18n/en-us/wiki/2023-12-30-ros2-tutorial/ch14-5-优化日志","Optimize logs","14.5",14050000,{"path":4955,"stem":4972,"title":5,"date":4958,"chapter":4956,"chapterSort":4957,"docKey":4960,"docRoot":4961,"docTitle":4962,"isWikiDoc":675,"isWikiIndex":4965},{"path":5147,"stem":5148,"title":5149,"date":4958,"chapter":5150,"chapterSort":5151,"docKey":4960,"docRoot":4961,"docTitle":4962,"isWikiDoc":675,"isWikiIndex":4965},"/en-us/wiki/2023-12-30-ros2-tutorial/ch14-6-2-guan-xing-ji-imu","_i18n/en-us/wiki/2023-12-30-ros2-tutorial/ch14-6-2-惯性计IMU","Inertial Measurement Unit (IMU)","14.6.2",14060200,{"path":5153,"stem":5154,"title":5155,"date":4958,"chapter":5156,"chapterSort":5157,"docKey":4960,"docRoot":4961,"docTitle":4962,"isWikiDoc":675,"isWikiIndex":4965},"/en-us/wiki/2023-12-30-ros2-tutorial/ch14-6-3-ji-guang-lei-da-lidar","_i18n/en-us/wiki/2023-12-30-ros2-tutorial/ch14-6-3-激光雷达LiDAR","LiDAR","14.6.3",14060300,{"path":5159,"stem":5160,"title":5161,"date":4958,"chapter":5162,"chapterSort":5163,"docKey":4960,"docRoot":4961,"docTitle":4962,"isWikiDoc":675,"isWikiIndex":4965},"/en-us/wiki/2023-12-30-ros2-tutorial/ch14-6-4-xiang-ji-camera","_i18n/en-us/wiki/2023-12-30-ros2-tutorial/ch14-6-4-相机Camera","Camera","14.6.4",14060400,{"path":5165,"stem":5166,"title":5167,"date":4958,"chapter":5168,"chapterSort":5169,"docKey":4960,"docRoot":4961,"docTitle":4962,"isWikiDoc":675,"isWikiIndex":4965},"/en-us/wiki/2023-12-30-ros2-tutorial/ch14-6-5-quan-qiu-ding-wei-gnss","_i18n/en-us/wiki/2023-12-30-ros2-tutorial/ch14-6-5-全球定位GNSS","Global Navigation Satellite System (GNSS)","14.6.5",14060500,{"path":5171,"stem":5172,"title":5173,"date":4958,"chapter":5174,"chapterSort":5175,"docKey":4960,"docRoot":4961,"docTitle":4962,"isWikiDoc":675,"isWikiIndex":4965},"/en-us/wiki/2023-12-30-ros2-tutorial/ch14-6-6-shou-bing-joy","_i18n/en-us/wiki/2023-12-30-ros2-tutorial/ch14-6-6-手柄joy","The query \"手柄JOY\" is a bit ambiguous. To help you better, could you please provide more details? For example:","14.6.6",14060600,{"path":5177,"stem":5178,"title":5179,"date":4958,"chapter":5180,"chapterSort":5181,"docKey":4960,"docRoot":4961,"docTitle":4962,"isWikiDoc":675,"isWikiIndex":4965},"/en-us/wiki/2023-12-30-ros2-tutorial/ch14-6-7-jian-pan-kong-zhi-jie-dian","_i18n/en-us/wiki/2023-12-30-ros2-tutorial/ch14-6-7-键盘控制节点","Keyboard control node","14.6.7",14060700,{"path":5183,"stem":5184,"title":5185,"date":4958,"chapter":5186,"chapterSort":5187,"docKey":4960,"docRoot":4961,"docTitle":4962,"isWikiDoc":675,"isWikiIndex":4965},"/en-us/wiki/2023-12-30-ros2-tutorial/ch14-6-ying-jian-ping-tai","_i18n/en-us/wiki/2023-12-30-ros2-tutorial/ch14-6-硬件平台","Hardware platform","14.6",14060000,{"path":5189,"stem":5190,"title":5191,"date":4958,"chapter":5192,"chapterSort":5193,"docKey":4960,"docRoot":4961,"docTitle":4962,"isWikiDoc":675,"isWikiIndex":4965},"/en-us/wiki/2023-12-30-ros2-tutorial/ch14-7-zuo-biao-xi-yu-hua-ti-guan-xi","_i18n/en-us/wiki/2023-12-30-ros2-tutorial/ch14-7-坐标系与话题关系","Coordinate System and Topic Relationship","14.7",14070000,{"path":5195,"stem":5196,"title":5197,"date":4958,"chapter":5198,"chapterSort":5199,"docKey":4960,"docRoot":4961,"docTitle":4962,"isWikiDoc":675,"isWikiIndex":4965},"/en-us/wiki/2023-12-30-ros2-tutorial/ch14-8-1-1-lun-shi-li-cheng-ji-biao-ding","_i18n/en-us/wiki/2023-12-30-ros2-tutorial/ch14-8-1-1-轮式里程计标定","Wheel odometry calibration","14.8.1.1",14080101,{"path":5201,"stem":5202,"title":5203,"date":4958,"chapter":5204,"chapterSort":5205,"docKey":4960,"docRoot":4961,"docTitle":4962,"isWikiDoc":675,"isWikiIndex":4965},"/en-us/wiki/2023-12-30-ros2-tutorial/ch14-8-1-2-lun-shi-li-cheng-ji-yu-imu-rong-he","_i18n/en-us/wiki/2023-12-30-ros2-tutorial/ch14-8-1-2-轮式里程计与IMU融合","Fusion of Wheel Odometry and IMU","14.8.1.2",14080102,{"path":5207,"stem":5208,"title":5209,"date":4958,"chapter":5210,"chapterSort":5211,"docKey":4960,"docRoot":4961,"docTitle":4962,"isWikiDoc":675,"isWikiIndex":4965},"/en-us/wiki/2023-12-30-ros2-tutorial/ch14-8-1-lun-shi-li-cheng-ji-biao-ding-yu-rong-he","_i18n/en-us/wiki/2023-12-30-ros2-tutorial/ch14-8-1-轮式里程计标定与融合","Wheel odometry calibration and fusion","14.8.1",14080100,{"path":5213,"stem":5214,"title":5215,"date":4958,"chapter":5216,"chapterSort":5217,"docKey":4960,"docRoot":4961,"docTitle":4962,"isWikiDoc":675,"isWikiIndex":4965},"/en-us/wiki/2023-12-30-ros2-tutorial/ch14-8-2-ji-guang-lei-da-gong-ju","_i18n/en-us/wiki/2023-12-30-ros2-tutorial/ch14-8-2-激光雷达工具","Lidar Tool","14.8.2",14080200,{"path":5219,"stem":5220,"title":5221,"date":4958,"chapter":5222,"chapterSort":5223,"docKey":4960,"docRoot":4961,"docTitle":4962,"isWikiDoc":675,"isWikiIndex":4965},"/en-us/wiki/2023-12-30-ros2-tutorial/ch14-8-3-xiang-ji-shi-yong-jin-jie","_i18n/en-us/wiki/2023-12-30-ros2-tutorial/ch14-8-3-相机使用进阶","Advanced Camera Usage","14.8.3",14080300,{"path":5225,"stem":5226,"title":5227,"date":4958,"chapter":5228,"chapterSort":5229,"docKey":4960,"docRoot":4961,"docTitle":4962,"isWikiDoc":675,"isWikiIndex":4965},"/en-us/wiki/2023-12-30-ros2-tutorial/ch14-8-ying-jian-ping-tai-jin-jie","_i18n/en-us/wiki/2023-12-30-ros2-tutorial/ch14-8-硬件平台进阶","Advanced Hardware Platform","14.8",14080000,{"path":5231,"stem":5232,"title":5233,"date":4958,"chapter":5234,"chapterSort":5235,"docKey":4960,"docRoot":4961,"docTitle":4962,"isWikiDoc":675,"isWikiIndex":4965},"/en-us/wiki/2023-12-30-ros2-tutorial/ch14-ji-qi-ren-ying-jian","_i18n/en-us/wiki/2023-12-30-ros2-tutorial/ch14-机器人硬件","Robot hardware","14",14000000,{"path":5237,"stem":5238,"title":5239,"date":4958,"chapter":5240,"chapterSort":5241,"docKey":4960,"docRoot":4961,"docTitle":4962,"isWikiDoc":675,"isWikiIndex":4965},"/en-us/wiki/2023-12-30-ros2-tutorial/ch15-ros2-control","_i18n/en-us/wiki/2023-12-30-ros2-tutorial/ch15-ROS2_Control","ROS2_Control","15",15000000,{"path":5243,"stem":5244,"title":5245,"date":4958,"chapter":5246,"chapterSort":5247,"docKey":4960,"docRoot":4961,"docTitle":4962,"isWikiDoc":675,"isWikiIndex":4965},"/en-us/wiki/2023-12-30-ros2-tutorial/ch16-moveit2-gong-ye-ji-qi-ren-ji-xie-bi","_i18n/en-us/wiki/2023-12-30-ros2-tutorial/ch16-Moveit2工业机器人机械臂","MoveIt2 industrial robot arm","16",16000000,{"path":5249,"stem":5250,"title":5251,"date":4958,"chapter":5252,"chapterSort":5253,"docKey":4960,"docRoot":4961,"docTitle":4962,"isWikiDoc":675,"isWikiIndex":4965},"/en-us/wiki/2023-12-30-ros2-tutorial/ch17-ji-qi-ren-dao-hang-navigation2-shi-ti-pian","_i18n/en-us/wiki/2023-12-30-ros2-tutorial/ch17-机器人导航Navigation2(实体篇)","Robot Navigation with Navigation2 (Physical Robot)","17",17000000,{"path":5255,"stem":5256,"title":5257,"date":4958,"chapter":5258,"chapterSort":5259,"docKey":4960,"docRoot":4961,"docTitle":4962,"isWikiDoc":675,"isWikiIndex":4965},"/en-us/wiki/2023-12-30-ros2-tutorial/ch18-microros","_i18n/en-us/wiki/2023-12-30-ros2-tutorial/ch18-MicroROS","MicroROS","18",18000000,{"path":5261,"stem":5262,"title":5263,"date":4958,"chapter":5264,"chapterSort":5265,"docKey":4960,"docRoot":4961,"docTitle":4962,"isWikiDoc":675,"isWikiIndex":4965},"/en-us/wiki/2023-12-30-ros2-tutorial/ch19-webots-fang-zhen-ping-tai","_i18n/en-us/wiki/2023-12-30-ros2-tutorial/ch19-Webots仿真平台","Webots simulation platform","19",19000000,{"path":5267,"stem":5268,"title":5269,"date":4958,"chapter":927,"chapterSort":5270,"docKey":4960,"docRoot":4961,"docTitle":4962,"isWikiDoc":675,"isWikiIndex":4965},"/en-us/wiki/2023-12-30-ros2-tutorial/ch2-ru-men-cao-zuo","_i18n/en-us/wiki/2023-12-30-ros2-tutorial/ch2-入门操作","Getting Started",2000000,{"path":5272,"stem":5273,"title":5274,"date":4958,"chapter":5275,"chapterSort":5276,"docKey":4960,"docRoot":4961,"docTitle":4962,"isWikiDoc":675,"isWikiIndex":4965},"/en-us/wiki/2023-12-30-ros2-tutorial/ch20-opencv","_i18n/en-us/wiki/2023-12-30-ros2-tutorial/ch20-OpenCV","OpenCV","20",20000000,{"path":5278,"stem":5279,"title":5280,"date":4958,"chapter":936,"chapterSort":5281,"docKey":4960,"docRoot":4961,"docTitle":4962,"isWikiDoc":675,"isWikiIndex":4965},"/en-us/wiki/2023-12-30-ros2-tutorial/ch3-gong-zuo-kong-jian-yu-gong-neng-bao","_i18n/en-us/wiki/2023-12-30-ros2-tutorial/ch3-工作空间与功能包","Workspace and Function Packages",3000000,{"path":5283,"stem":5284,"title":5285,"date":4958,"chapter":3162,"chapterSort":5286,"docKey":4960,"docRoot":4961,"docTitle":4962,"isWikiDoc":675,"isWikiIndex":4965},"/en-us/wiki/2023-12-30-ros2-tutorial/ch4-si-da-tong-xin","_i18n/en-us/wiki/2023-12-30-ros2-tutorial/ch4-四大通信","Four Major Communications",4000000,{"path":5288,"stem":5289,"title":5290,"date":4958,"chapter":3177,"chapterSort":5291,"docKey":4960,"docRoot":4961,"docTitle":4962,"isWikiDoc":675,"isWikiIndex":4965},"/en-us/wiki/2023-12-30-ros2-tutorial/ch5-ros2-qi-ta-tong-xin-ji-zhi","_i18n/en-us/wiki/2023-12-30-ros2-tutorial/ch5-ROS2其他通信机制","ROS2 Other Communication Mechanisms",5000000,{"path":5293,"stem":5294,"title":5295,"date":4958,"chapter":3192,"chapterSort":5296,"docKey":4960,"docRoot":4961,"docTitle":4962,"isWikiDoc":675,"isWikiIndex":4965},"/en-us/wiki/2023-12-30-ros2-tutorial/ch6-launch","_i18n/en-us/wiki/2023-12-30-ros2-tutorial/ch6-Launch","Launch",6000000,{"path":5298,"stem":5299,"title":5300,"date":4958,"chapter":5301,"chapterSort":5302,"docKey":4960,"docRoot":4961,"docTitle":4962,"isWikiDoc":675,"isWikiIndex":4965},"/en-us/wiki/2023-12-30-ros2-tutorial/ch7-hui-su-rosbag2","_i18n/en-us/wiki/2023-12-30-ros2-tutorial/ch7-回溯rosbag2","Replaying rosbag2","7",7000000,{"path":5304,"stem":5305,"title":5306,"date":4958,"chapter":5307,"chapterSort":5308,"docKey":4960,"docRoot":4961,"docTitle":4962,"isWikiDoc":675,"isWikiIndex":4965},"/en-us/wiki/2023-12-30-ros2-tutorial/ch8-zuo-biao-bian-huan-tf","_i18n/en-us/wiki/2023-12-30-ros2-tutorial/ch8-坐标变换TF","Coordinate Transformation TF","8",8000000,{"path":5310,"stem":5311,"title":5312,"date":4958,"chapter":5313,"chapterSort":5314,"docKey":4960,"docRoot":4961,"docTitle":4962,"isWikiDoc":675,"isWikiIndex":4965},"/en-us/wiki/2023-12-30-ros2-tutorial/ch9-ke-shi-hua-ping-tai-rviz2-yu-urdf-jian-mo-yu-yan","_i18n/en-us/wiki/2023-12-30-ros2-tutorial/ch9-可视化平台RVIZ2与URDF建模语言","Visualization Platform RVIZ2 and URDF Modeling Language","9",9000000,{"path":4961,"stem":5316,"title":5317,"date":4958,"chapter":4966,"chapterSort":5318,"docKey":4960,"docRoot":4961,"docTitle":4962,"isWikiDoc":675,"isWikiIndex":675},"_i18n/en-us/wiki/2023-12-30-ros2-tutorial/index","ROS2 Robot Operating System Tutorial",0,{"variants":5320},[5321,5322,5325,5328,5331],{"path":4955,"localeSlug":4968,"i18nKey":4964},{"path":5323,"localeSlug":5324,"i18nKey":4964},"/zh-hant/wiki/2023-12-30-ros2-tutorial/ch14-6-1-li-cheng-ji-odom","zh-hant",{"path":5326,"localeSlug":5327,"i18nKey":4964},"/zh-hk/wiki/2023-12-30-ros2-tutorial/ch14-6-1-li-cheng-ji-odom","zh-hk",{"path":5329,"localeSlug":5330,"i18nKey":4964},"/zh-tw/wiki/2023-12-30-ros2-tutorial/ch14-6-1-li-cheng-ji-odom","zh-tw",{"path":5332,"localeSlug":5333,"i18nKey":4964},"/zh-cn/wiki/2023-12-30-ros2-tutorial/ch14-6-1-li-cheng-ji-odom","zh-cn",[4955,4971,5323,4971,5326,4971,5329,4971,5332,4971],1780671804937]