[已解决]C++问题求助！！

Freedy_S

有没有大佬告诉我一下这个代码写的是啥意思 求助！！顺便大致标注一下～谢谢！

1. #include"utility.h"
   
2. 
   
3. 
   
4. class ConversionPosion
   
5. {
   
6. public:
   
7.    
   
8.     ConversionPosion()
   
9.     {   
   
10.         SubOdometry = nh.subscribe<nav_msgs::Odometry>("/integrated_to_init",5,&ConversionPosion::OdometryHandle,this);
    
11.         pubOdometry = nh.advertise<geometry_msgs::Pose2D>("/my_position",5);
    
12.     }
    
13. 
    
14.     void OdometryHandle(const nav_msgs::Odometry::ConstPtr &msg)
    
15.     {
    
16.         currentHeader = msg->header;
    
17.         
    
18.         double roll,pitch,yaw;
    
19. 
    
20.         geometry_msgs::Quaternion geoQut = msg->pose.pose.orientation;
    
21.         tf::Matrix3x3(tf::Quaternion(geoQut.z, -geoQut.x, -geoQut.y, geoQut.w)).getRPY(roll,pitch,yaw);
    
22. 
    
23.         My_Position.theta = (yaw/PI)*180;
    
24.         My_Position.x = msg->pose.pose.position.x;
    
25.         My_Position.y = msg->pose.pose.position.y;
    
26. 
    
27.         pubOdometry.publish(My_Position);
    
28. 
    
29.     }
    
30. 
    
31.     // void OdometryHandle(const nav_msgs::Odometry::ConstPtr &msg)
    
32.     // {
    
33.     //     geometry_msgs::PoseStamped thispose;
    
34.     //     thispose.header.frame_id = msg->header.frame_id;
    
35.     //     thispose.header.stamp = ros::Time::now();
    
36.     //     thispose.pose.position.x = msg->pose.pose.position.x;
    
37.     //     thispose.pose.position.y = msg->pose.pose.position.y;
    
38.     //     thispose.pose.position.z = msg->pose.pose.position.z;
    
39.     //     thispose.pose.orientation = msg->pose.pose.orientation;
    
40. 
    
41.     //     path.poses.push_back(thispose);
    
42.     //     path.header.frame_id = msg->header.frame_id;
    
43.     //     path.header.stamp = ros::Time::now();
    
44. 
    
45.     //     pubOdometry.publish(path);
    
46. 
    
47.     // }
    
48.    
    
49.    
    
50. 
    
51. private:
    
52.     ros::NodeHandle nh;
    
53.     ros::Subscriber SubOdometry;
    
54.     ros::Publisher pubOdometry;
    
55.    
    
56.     std_msgs::Header currentHeader;
    
57. 
    
58.     geometry_msgs::Pose2D My_Position;
    
59.     nav_msgs::Path path;
    
60. };
    
61. 
    
62. 
    
63. int main(int argc, char *argv[])
    
64. {
    
65.     ros::init(argc, argv, "lego_loam");
    
66.    
    
67.     ConversionPosion Cp;
    
68. 
    
69.     ROS_INFO("\033[1;32m---->\033[0m  ConversionPosion Started.");
    
70. 
    
71.     ros::spin();
    
72.    
    
73.     return 0;
    
74. }

复制代码

最佳答案

月排行榜 / 总排行榜

lvk

2022-12-31 15:48:37

大概就是定义了一个用来订阅机器人nav_msgs::Odometry消息，然后经OdometryHandle函数转换为geometry_msgs::Pose2D消息并发布出去的类；在主函数里初始化了ros并定义一个那个类的对象，然后打印了一下信息，最后进入spin,此时机器人会不断订阅nav_msgs::Odometry消息，发布geometry_msgs::Pose2D消息。

跳转到最佳答案楼层

Freedy_S

这是头文件

1. #ifndef _UTILITY_LIDAR_ODOMETRY_H_
   
2. #define _UTILITY_LIDAR_ODOMETRY_H_
   
3. 
   
4. 
   
5. #include <ros/ros.h>
   
6. 
   
7. #include <sensor_msgs/Imu.h>
   
8. #include <sensor_msgs/PointCloud2.h>
   
9. #include <nav_msgs/Odometry.h>
   
10. #include<nav_msgs/Path.h>
    
11. #include<geometry_msgs/PoseStamped.h>
    
12. #include<geometry_msgs/Pose2D.h>
    
13. #include <geometry_msgs/Quaternion.h>
    
14. #include "cloud_msgs/cloud_info.h"
    
15. 
    
16. #include <opencv/cv.h>
    
17. 
    
18. #include <pcl/point_cloud.h>
    
19. #include <pcl/point_types.h>
    
20. #include <pcl_ros/point_cloud.h>
    
21. #include <pcl_conversions/pcl_conversions.h>
    
22. #include <pcl/range_image/range_image.h>
    
23. #include <pcl/filters/filter.h>
    
24. #include <pcl/filters/voxel_grid.h>
    
25. #include <pcl/kdtree/kdtree_flann.h>
    
26. #include <pcl/common/common.h>
    
27. #include <pcl/registration/icp.h>
    
28. 
    
29. #include <tf/transform_broadcaster.h>
    
30. #include <tf/transform_datatypes.h>
    
31. 
    
32. #include <vector>
    
33. #include <cmath>
    
34. #include <algorithm>
    
35. #include <queue>
    
36. #include <deque>
    
37. #include <iostream>
    
38. #include <fstream>
    
39. #include <ctime>
    
40. #include <cfloat>
    
41. #include <iterator>
    
42. #include <sstream>
    
43. #include <string>
    
44. #include <limits>
    
45. #include <iomanip>
    
46. #include <array>
    
47. #include <thread>
    
48. #include <mutex>
    
49. 
    
50. #define PI 3.14159265
    
51. 
    
52. using namespace std;
    
53. 
    
54. typedef pcl::PointXYZI  PointType;
    
55. 
    
56. extern const string pointCloudTopic = "/lidar_points";
    
57. extern const string imuTopic = "";
    
58. 
    
59. // Save pcd
    
60. extern const string fileDirectory = "/home/lizhiyuan/";
    
61. 
    
62. // Using velodyne cloud "ring" channel for image projection (other lidar may have different name for this channel, change "PointXYZIR" below)
    
63. extern const bool useCloudRing = false; // if true, ang_res_y and ang_bottom are not used
    
64. 
    
65. // VLP-16
    
66. //extern const int N_SCAN = 16;
    
67. //extern const int Horizon_SCAN = 1800;
    
68. //extern const float ang_res_x = 0.2;
    
69. //extern const float ang_res_y = 2.0;
    
70. //extern const float ang_bottom = 15.0+0.1;
    
71. //extern const int groundScanInd = 7;
    
72. 
    
73. // HDL-32E
    
74. // extern const int N_SCAN = 32;
    
75. // extern const int Horizon_SCAN = 1800;
    
76. // extern const float ang_res_x = 360.0/float(Horizon_SCAN);
    
77. // extern const float ang_res_y = 41.33/float(N_SCAN-1);
    
78. // extern const float ang_bottom = 30.67;
    
79. // extern const int groundScanInd = 20;
    
80. 
    
81. //C-Fans-32
    
82. // extern const int N_SCAN = 32;
    
83. // extern const int Horizon_SCAN =600;
    
84. // extern const float ang_res_x = 0.2;
    
85. // extern const float ang_res_y =0.6;
    
86. // extern const float ang_bottom = 10.2;
    
87. // extern const int groundScanInd = 10;
    
88. 
    
89. //R-Fans-32
    
90. // extern const int N_SCAN = 32;
    
91. // extern const int Horizon_SCAN =2000;
    
92. // extern const float ang_res_x = 0.18;
    
93. // extern const float ang_res_y =1;
    
94. // extern const float ang_bottom = 20;
    
95. // extern const int groundScanInd = 10;
    
96. //R-fans-32M
    
97. extern const int N_SCAN = 32;
    
98. extern const int Horizon_SCAN =2000;
    
99. extern const float ang_res_x = 0.18;
    
100. //extern const float ang_res_y =0.87;
     
101. extern const float ang_bottom = 16.9;
     
102. extern const int groundScanInd = 10;
     
103. //HDL-64
     
104. // extern const int N_SCAN = 64;
     
105. // extern const int Horizon_SCAN =4000;
     
106. // extern const float ang_res_x = 0.09;
     
107. // extern const float ang_res_y =0.4;
     
108. // extern const float ang_bottom = 24.8;
     
109. // extern const int groundScanInd = 20;
     
110. 
     
111. 
     
112. 
     
113. // VLS-128
     
114. // extern const int N_SCAN = 128;
     
115. // extern const int Horizon_SCAN = 1800;
     
116. // extern const float ang_res_x = 0.2;
     
117. // extern const float ang_res_y = 0.3;
     
118. // extern const float ang_bottom = 25.0;
     
119. // extern const int groundScanInd = 10;
     
120. 
     
121. // Ouster users may need to uncomment line 159 in imageProjection.cpp
     
122. // Usage of Ouster imu data is not fully supported yet (LeGO-LOAM needs 9-DOF IMU), please just publish point cloud data
     
123. // Ouster OS1-16
     
124. // extern const int N_SCAN = 16;
     
125. // extern const int Horizon_SCAN = 1024;
     
126. // extern const float ang_res_x = 360.0/float(Horizon_SCAN);
     
127. // extern const float ang_res_y = 33.2/float(N_SCAN-1);
     
128. // extern const float ang_bottom = 16.6+0.1;
     
129. // extern const int groundScanInd = 7;
     
130. 
     
131. // Ouster OS1-64
     
132. // extern const int N_SCAN = 64;
     
133. // extern const int Horizon_SCAN = 1024;
     
134. // extern const float ang_res_x = 360.0/float(Horizon_SCAN);
     
135. // extern const float ang_res_y = 33.2/float(N_SCAN-1);
     
136. // extern const float ang_bottom = 16.6+0.1;
     
137. // extern const int groundScanInd = 15;
     
138. 
     
139. extern const bool loopClosureEnableFlag = false;
     
140. extern const double mappingProcessInterval = 0.3;
     
141. 
     
142. extern const float scanPeriod = 0.1;
     
143. extern const int systemDelay = 0;
     
144. extern const int imuQueLength = 200;
     
145. 
     
146. extern const float sensorMinimumRange = 1.0;
     
147. extern const float sensorMountAngle = 0.0;
     
148. extern const float segmentTheta = 60.0/180.0*M_PI; // decrese this value may improve accuracy
     
149. extern const int segmentValidPointNum = 5;
     
150. extern const int segmentValidLineNum = 3;
     
151. extern const float segmentAlphaX = ang_res_x / 180.0 * M_PI;
     
152. extern const float segmentAlphaY = 1/ 180.0 * M_PI;
     
153. 
     
154. 
     
155. extern const int edgeFeatureNum = 2;
     
156. extern const int surfFeatureNum = 4;
     
157. extern const int sectionsTotal = 6;
     
158. extern const float edgeThreshold = 0.1;
     
159. extern const float surfThreshold = 0.1;
     
160. extern const float nearestFeatureSearchSqDist = 25;
     
161. 
     
162. 
     
163. // Mapping Params
     
164. extern const float surroundingKeyframeSearchRadius = 50.0; // key frame that is within n meters from current pose will be considerd for scan-to-map optimization (when loop closure disabled)
     
165. extern const int   surroundingKeyframeSearchNum = 50; // submap size (when loop closure enabled)
     
166. // history key frames (history submap for loop closure)
     
167. extern const float historyKeyframeSearchRadius = 7.0; // key frame that is within n meters from current pose will be considerd for loop closure
     
168. extern const int   historyKeyframeSearchNum = 25; // 2n+1 number of hostory key frames will be fused into a submap for loop closure
     
169. extern const float historyKeyframeFitnessScore = 0.3; // the smaller the better alignment
     
170. 
     
171. extern const float globalMapVisualizationSearchRadius = 500.0; // key frames with in n meters will be visualized
     
172. 
     
173. 
     
174. struct smoothness_t{
     
175.     float value;
     
176.     size_t ind;
     
177. };
     
178. 
     
179. struct by_value{
     
180.     bool operator()(smoothness_t const &left, smoothness_t const &right) {
     
181.         return left.value < right.value;
     
182.     }
     
183. };
     
184. 
     
185. /*
     
186.     * A point cloud type that has "ring" channel
     
187.     */
     
188. struct PointXYZIR
     
189. {
     
190.     PCL_ADD_POINT4D
     
191.     PCL_ADD_INTENSITY;
     
192.     uint16_t ring;
     
193.     EIGEN_MAKE_ALIGNED_OPERATOR_NEW
     
194. } EIGEN_ALIGN16;
     
195. 
     
196. POINT_CLOUD_REGISTER_POINT_STRUCT (PointXYZIR,  
     
197.                                    (float, x, x) (float, y, y)
     
198.                                    (float, z, z) (float, intensity, intensity)
     
199.                                    (uint16_t, ring, ring)
     
200. )
     
201. 
     
202. /*
     
203.     * A point cloud type that has 6D pose info ([x,y,z,roll,pitch,yaw] intensity is time stamp)
     
204.     */
     
205. struct PointXYZIRPYT
     
206. {
     
207.     PCL_ADD_POINT4D
     
208.     PCL_ADD_INTENSITY;
     
209.     float roll;
     
210.     float pitch;
     
211.     float yaw;
     
212.     double time;
     
213.     EIGEN_MAKE_ALIGNED_OPERATOR_NEW
     
214. } EIGEN_ALIGN16;
     
215. 
     
216. POINT_CLOUD_REGISTER_POINT_STRUCT (PointXYZIRPYT,
     
217.                                    (float, x, x) (float, y, y)
     
218.                                    (float, z, z) (float, intensity, intensity)
     
219.                                    (float, roll, roll) (float, pitch, pitch) (float, yaw, yaw)
     
220.                                    (double, time, time)
     
221. )
     
222. 
     
223. typedef PointXYZIRPYT  PointTypePose;
     
224. 
     
225. #endif
     

复制代码

lvk

学一下ROS就知道了

Freedy_S

lvk 发表于 2022-12-30 23:34
学一下ROS就知道了

那这段大致是啥意思大佬！

lvk

大概就是定义了一个用来订阅机器人nav_msgs::Odometry消息，然后经OdometryHandle函数转换为geometry_msgs::Pose2D消息并发布出去的类；在主函数里初始化了ros并定义一个那个类的对象，然后打印了一下信息，最后进入spin,此时机器人会不断订阅nav_msgs::Odometry消息，发布geometry_msgs::Pose2D消息。