-
Notifications
You must be signed in to change notification settings - Fork 102
/
main.cc
135 lines (123 loc) · 4.6 KB
/
main.cc
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
#include <ros/ros.h>
#include <image_transport/image_transport.h>
#include <sensor_msgs/PointCloud2.h>
#include <pcl_conversions/pcl_conversions.h>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include "pandarGeneral_sdk/pandarGeneral_sdk.h"
using namespace std;
class HesaiLidarClient
{
public:
HesaiLidarClient(ros::NodeHandle node, ros::NodeHandle nh)
{
lidarPublisher = node.advertise<sensor_msgs::PointCloud2>("pandar", 10);
packetPublisher = node.advertise<hesai_lidar::PandarScan>("pandar_packets",10);
string serverIp;
int lidarRecvPort;
int gpsPort;
double startAngle;
string lidarCorrectionFile; // Get local correction when getting from lidar failed
string lidarType;
string frameId;
int pclDataType;
string pcapFile;
string dataType;
nh.getParam("pcap_file", pcapFile);
nh.getParam("server_ip", serverIp);
nh.getParam("lidar_recv_port", lidarRecvPort);
nh.getParam("gps_port", gpsPort);
nh.getParam("start_angle", startAngle);
nh.getParam("lidar_correction_file", lidarCorrectionFile);
nh.getParam("lidar_type", lidarType);
nh.getParam("frame_id", frameId);
nh.getParam("pcldata_type", pclDataType);
nh.getParam("publish_type", m_sPublishType);
nh.getParam("timestamp_type", m_sTimestampType);
nh.getParam("data_type", dataType);
if(!pcapFile.empty()){
hsdk = new PandarGeneralSDK(pcapFile, boost::bind(&HesaiLidarClient::lidarCallback, this, _1, _2, _3), \
static_cast<int>(startAngle * 100 + 0.5), 0, pclDataType, lidarType, frameId, m_sTimestampType);
if (hsdk != NULL) {
ifstream fin(lidarCorrectionFile);
int length = 0;
std::string strlidarCalibration;
fin.seekg(0, std::ios::end);
length = fin.tellg();
fin.seekg(0, std::ios::beg);
char *buffer = new char[length];
fin.read(buffer, length);
fin.close();
strlidarCalibration = buffer;
hsdk->LoadLidarCorrectionFile(strlidarCalibration);
}
}
else if ("rosbag" == dataType){
hsdk = new PandarGeneralSDK("", boost::bind(&HesaiLidarClient::lidarCallback, this, _1, _2, _3), \
static_cast<int>(startAngle * 100 + 0.5), 0, pclDataType, lidarType, frameId, m_sTimestampType);
if (hsdk != NULL) {
ifstream fin(lidarCorrectionFile);
int length = 0;
std::string strlidarCalibration;
fin.seekg(0, std::ios::end);
length = fin.tellg();
fin.seekg(0, std::ios::beg);
char *buffer = new char[length];
fin.read(buffer, length);
fin.close();
strlidarCalibration = buffer;
hsdk->LoadLidarCorrectionFile(strlidarCalibration);
packetSubscriber = node.subscribe("pandar_packets",10,&HesaiLidarClient::scanCallback, (HesaiLidarClient*)this, ros::TransportHints().tcpNoDelay(true));
}
}
else {
hsdk = new PandarGeneralSDK(serverIp, lidarRecvPort, gpsPort, \
boost::bind(&HesaiLidarClient::lidarCallback, this, _1, _2, _3), \
boost::bind(&HesaiLidarClient::gpsCallback, this, _1), static_cast<int>(startAngle * 100 + 0.5), 0, pclDataType, lidarType, frameId, m_sTimestampType);
}
if (hsdk != NULL) {
hsdk->Start();
// hsdk->LoadLidarCorrectionFile("..."); // parameter is stream in lidarCorrectionFile
} else {
printf("create sdk fail\n");
}
}
void lidarCallback(boost::shared_ptr<PPointCloud> cld, double timestamp, hesai_lidar::PandarScanPtr scan) // the timestamp from first point cloud of cld
{
if(m_sPublishType == "both" || m_sPublishType == "points"){
pcl_conversions::toPCL(ros::Time(timestamp), cld->header.stamp);
sensor_msgs::PointCloud2 output;
pcl::toROSMsg(*cld, output);
lidarPublisher.publish(output);
printf("timestamp: %f, point size: %ld.\n",timestamp, cld->points.size());
}
if(m_sPublishType == "both" || m_sPublishType == "raw"){
packetPublisher.publish(scan);
printf("raw size: %d.\n", scan->packets.size());
}
}
void gpsCallback(int timestamp) {
printf("gps: %d\n", timestamp);
}
void scanCallback(const hesai_lidar::PandarScanPtr scan)
{
// printf("pandar_packets topic message received,\n");
hsdk->PushScanPacket(scan);
}
private:
ros::Publisher lidarPublisher;
ros::Publisher packetPublisher;
PandarGeneralSDK* hsdk;
string m_sPublishType;
string m_sTimestampType;
ros::Subscriber packetSubscriber;
};
int main(int argc, char **argv)
{
ros::init(argc, argv, "pandar");
ros::NodeHandle nh("~");
ros::NodeHandle node;
HesaiLidarClient pandarClient(node, nh);
ros::spin();
return 0;
}