C++问题求助!!
有没有大佬告诉我一下这个代码写的是啥意思 求助!!顺便大致标注一下~谢谢!#include"utility.h"
class ConversionPosion
{
public:
ConversionPosion()
{
SubOdometry = nh.subscribe<nav_msgs::Odometry>("/integrated_to_init",5,&ConversionPosion::OdometryHandle,this);
pubOdometry = nh.advertise<geometry_msgs::Pose2D>("/my_position",5);
}
void OdometryHandle(const nav_msgs::Odometry::ConstPtr &msg)
{
currentHeader = msg->header;
double roll,pitch,yaw;
geometry_msgs::Quaternion geoQut = msg->pose.pose.orientation;
tf::Matrix3x3(tf::Quaternion(geoQut.z, -geoQut.x, -geoQut.y, geoQut.w)).getRPY(roll,pitch,yaw);
My_Position.theta = (yaw/PI)*180;
My_Position.x = msg->pose.pose.position.x;
My_Position.y = msg->pose.pose.position.y;
pubOdometry.publish(My_Position);
}
// void OdometryHandle(const nav_msgs::Odometry::ConstPtr &msg)
// {
// geometry_msgs::PoseStamped thispose;
// thispose.header.frame_id = msg->header.frame_id;
// thispose.header.stamp = ros::Time::now();
// thispose.pose.position.x = msg->pose.pose.position.x;
// thispose.pose.position.y = msg->pose.pose.position.y;
// thispose.pose.position.z = msg->pose.pose.position.z;
// thispose.pose.orientation = msg->pose.pose.orientation;
// path.poses.push_back(thispose);
// path.header.frame_id = msg->header.frame_id;
// path.header.stamp = ros::Time::now();
// pubOdometry.publish(path);
// }
private:
ros::NodeHandle nh;
ros::Subscriber SubOdometry;
ros::Publisher pubOdometry;
std_msgs::Header currentHeader;
geometry_msgs::Pose2D My_Position;
nav_msgs::Path path;
};
int main(int argc, char *argv[])
{
ros::init(argc, argv, "lego_loam");
ConversionPosion Cp;
ROS_INFO("\033[1;32m---->\033[0mConversionPosion Started.");
ros::spin();
return 0;
} 这是头文件
#ifndef _UTILITY_LIDAR_ODOMETRY_H_
#define _UTILITY_LIDAR_ODOMETRY_H_
#include <ros/ros.h>
#include <sensor_msgs/Imu.h>
#include <sensor_msgs/PointCloud2.h>
#include <nav_msgs/Odometry.h>
#include<nav_msgs/Path.h>
#include<geometry_msgs/PoseStamped.h>
#include<geometry_msgs/Pose2D.h>
#include <geometry_msgs/Quaternion.h>
#include "cloud_msgs/cloud_info.h"
#include <opencv/cv.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl_ros/point_cloud.h>
#include <pcl_conversions/pcl_conversions.h>
#include <pcl/range_image/range_image.h>
#include <pcl/filters/filter.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/kdtree/kdtree_flann.h>
#include <pcl/common/common.h>
#include <pcl/registration/icp.h>
#include <tf/transform_broadcaster.h>
#include <tf/transform_datatypes.h>
#include <vector>
#include <cmath>
#include <algorithm>
#include <queue>
#include <deque>
#include <iostream>
#include <fstream>
#include <ctime>
#include <cfloat>
#include <iterator>
#include <sstream>
#include <string>
#include <limits>
#include <iomanip>
#include <array>
#include <thread>
#include <mutex>
#define PI 3.14159265
using namespace std;
typedef pcl::PointXYZIPointType;
extern const string pointCloudTopic = "/lidar_points";
extern const string imuTopic = "";
// Save pcd
extern const string fileDirectory = "/home/lizhiyuan/";
// Using velodyne cloud "ring" channel for image projection (other lidar may have different name for this channel, change "PointXYZIR" below)
extern const bool useCloudRing = false; // if true, ang_res_y and ang_bottom are not used
// VLP-16
//extern const int N_SCAN = 16;
//extern const int Horizon_SCAN = 1800;
//extern const float ang_res_x = 0.2;
//extern const float ang_res_y = 2.0;
//extern const float ang_bottom = 15.0+0.1;
//extern const int groundScanInd = 7;
// HDL-32E
// extern const int N_SCAN = 32;
// extern const int Horizon_SCAN = 1800;
// extern const float ang_res_x = 360.0/float(Horizon_SCAN);
// extern const float ang_res_y = 41.33/float(N_SCAN-1);
// extern const float ang_bottom = 30.67;
// extern const int groundScanInd = 20;
//C-Fans-32
// extern const int N_SCAN = 32;
// extern const int Horizon_SCAN =600;
// extern const float ang_res_x = 0.2;
// extern const float ang_res_y =0.6;
// extern const float ang_bottom = 10.2;
// extern const int groundScanInd = 10;
//R-Fans-32
// extern const int N_SCAN = 32;
// extern const int Horizon_SCAN =2000;
// extern const float ang_res_x = 0.18;
// extern const float ang_res_y =1;
// extern const float ang_bottom = 20;
// extern const int groundScanInd = 10;
//R-fans-32M
extern const int N_SCAN = 32;
extern const int Horizon_SCAN =2000;
extern const float ang_res_x = 0.18;
//extern const float ang_res_y =0.87;
extern const float ang_bottom = 16.9;
extern const int groundScanInd = 10;
//HDL-64
// extern const int N_SCAN = 64;
// extern const int Horizon_SCAN =4000;
// extern const float ang_res_x = 0.09;
// extern const float ang_res_y =0.4;
// extern const float ang_bottom = 24.8;
// extern const int groundScanInd = 20;
// VLS-128
// extern const int N_SCAN = 128;
// extern const int Horizon_SCAN = 1800;
// extern const float ang_res_x = 0.2;
// extern const float ang_res_y = 0.3;
// extern const float ang_bottom = 25.0;
// extern const int groundScanInd = 10;
// Ouster users may need to uncomment line 159 in imageProjection.cpp
// Usage of Ouster imu data is not fully supported yet (LeGO-LOAM needs 9-DOF IMU), please just publish point cloud data
// Ouster OS1-16
// extern const int N_SCAN = 16;
// extern const int Horizon_SCAN = 1024;
// extern const float ang_res_x = 360.0/float(Horizon_SCAN);
// extern const float ang_res_y = 33.2/float(N_SCAN-1);
// extern const float ang_bottom = 16.6+0.1;
// extern const int groundScanInd = 7;
// Ouster OS1-64
// extern const int N_SCAN = 64;
// extern const int Horizon_SCAN = 1024;
// extern const float ang_res_x = 360.0/float(Horizon_SCAN);
// extern const float ang_res_y = 33.2/float(N_SCAN-1);
// extern const float ang_bottom = 16.6+0.1;
// extern const int groundScanInd = 15;
extern const bool loopClosureEnableFlag = false;
extern const double mappingProcessInterval = 0.3;
extern const float scanPeriod = 0.1;
extern const int systemDelay = 0;
extern const int imuQueLength = 200;
extern const float sensorMinimumRange = 1.0;
extern const float sensorMountAngle = 0.0;
extern const float segmentTheta = 60.0/180.0*M_PI; // decrese this value may improve accuracy
extern const int segmentValidPointNum = 5;
extern const int segmentValidLineNum = 3;
extern const float segmentAlphaX = ang_res_x / 180.0 * M_PI;
extern const float segmentAlphaY = 1/ 180.0 * M_PI;
extern const int edgeFeatureNum = 2;
extern const int surfFeatureNum = 4;
extern const int sectionsTotal = 6;
extern const float edgeThreshold = 0.1;
extern const float surfThreshold = 0.1;
extern const float nearestFeatureSearchSqDist = 25;
// Mapping Params
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)
extern const int surroundingKeyframeSearchNum = 50; // submap size (when loop closure enabled)
// history key frames (history submap for loop closure)
extern const float historyKeyframeSearchRadius = 7.0; // key frame that is within n meters from current pose will be considerd for loop closure
extern const int historyKeyframeSearchNum = 25; // 2n+1 number of hostory key frames will be fused into a submap for loop closure
extern const float historyKeyframeFitnessScore = 0.3; // the smaller the better alignment
extern const float globalMapVisualizationSearchRadius = 500.0; // key frames with in n meters will be visualized
struct smoothness_t{
float value;
size_t ind;
};
struct by_value{
bool operator()(smoothness_t const &left, smoothness_t const &right) {
return left.value < right.value;
}
};
/*
* A point cloud type that has "ring" channel
*/
struct PointXYZIR
{
PCL_ADD_POINT4D
PCL_ADD_INTENSITY;
uint16_t ring;
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
} EIGEN_ALIGN16;
POINT_CLOUD_REGISTER_POINT_STRUCT (PointXYZIR,
(float, x, x) (float, y, y)
(float, z, z) (float, intensity, intensity)
(uint16_t, ring, ring)
)
/*
* A point cloud type that has 6D pose info ( intensity is time stamp)
*/
struct PointXYZIRPYT
{
PCL_ADD_POINT4D
PCL_ADD_INTENSITY;
float roll;
float pitch;
float yaw;
double time;
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
} EIGEN_ALIGN16;
POINT_CLOUD_REGISTER_POINT_STRUCT (PointXYZIRPYT,
(float, x, x) (float, y, y)
(float, z, z) (float, intensity, intensity)
(float, roll, roll) (float, pitch, pitch) (float, yaw, yaw)
(double, time, time)
)
typedef PointXYZIRPYTPointTypePose;
#endif
学一下ROS就知道了 lvk 发表于 2022-12-30 23:34
学一下ROS就知道了
那这段大致是啥意思大佬! 大概就是定义了一个用来订阅机器人nav_msgs::Odometry消息,然后经OdometryHandle函数转换为geometry_msgs::Pose2D消息并发布出去的类;在主函数里初始化了ros并定义一个那个类的对象,然后打印了一下信息,最后进入spin,此时机器人会不断订阅nav_msgs::Odometry消息,发布geometry_msgs::Pose2D消息。
页:
[1]