Skip to content

Commit

Permalink
format codes
Browse files Browse the repository at this point in the history
  • Loading branch information
@0x126
0x126 committed Mar 25, 2021
1 parent a9da42c commit 7cdebc8
Show file tree
Hide file tree
Showing 26 changed files with 298 additions and 285 deletions.
2 changes: 1 addition & 1 deletion pandar_driver/include/pandar_driver/input.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -7,6 +7,6 @@ class Input
{
public:
virtual ~Input(){};
virtual int getPacket(pandar_msgs::PandarPacket * pkt) = 0;
virtual int getPacket(pandar_msgs::PandarPacket* pkt) = 0;
};
} // namespace pandar_driver
4 changes: 2 additions & 2 deletions pandar_driver/include/pandar_driver/pcap_input.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -27,7 +27,7 @@ class PcapInput : public Input
PcapInput(uint16_t port, uint16_t gps_port, std::string path, std::string model);
~PcapInput();

int getPacket(pandar_msgs::PandarPacket * pandar_pkt) override;
int getPacket(pandar_msgs::PandarPacket* pandar_pkt) override;

private:
void initTimeIndexMap();
Expand All @@ -37,7 +37,7 @@ class PcapInput : public Input
int utc_index_;
std::map<std::string, std::pair<int, int>> time_index_map_;

pcap_t * pcap_;
pcap_t* pcap_;
char errbuf_[PCAP_ERRBUF_SIZE];
bpf_program pcap_filter_;

Expand Down
2 changes: 1 addition & 1 deletion pandar_driver/include/pandar_driver/socket_input.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -27,7 +27,7 @@ class SocketInput : public Input
public:
SocketInput(uint16_t port, uint16_t gpsPort);
~SocketInput();
int getPacket(pandar_msgs::PandarPacket * pkt) override;
int getPacket(pandar_msgs::PandarPacket* pkt) override;

private:
int socketForLidar;
Expand Down
3 changes: 1 addition & 2 deletions pandar_driver/src/driver/node.cpp
View file
Original file line number Diff line number Diff line change
@@ -1,8 +1,7 @@
#include <ros/ros.h>
#include <pandar_driver/pandar_driver.h>


int main(int argc, char * argv[])
int main(int argc, char* argv[])
{
ros::init(argc, argv, "pandar_driver_node");
ros::NodeHandle node;
Expand Down
9 changes: 6 additions & 3 deletions pandar_driver/src/driver/nodelet.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -11,13 +11,15 @@ namespace pandar_driver
class DriverNodelet : public nodelet::Nodelet
{
public:
DriverNodelet() : running_(false) {}
DriverNodelet() : running_(false)
{
}

~DriverNodelet()
{
NODELET_INFO("shutting down driver thread");
running_ = false;
if(deviceThread_.joinable()){
if (deviceThread_.joinable()) {
deviceThread_.join();
}
NODELET_INFO("driver thread stopped");
Expand All @@ -44,7 +46,8 @@ void DriverNodelet::devicePoll()
{
while (ros::ok()) {
running_ = driver_->poll();
if (!running_) break;
if (!running_)
break;
}
running_ = false;
}
Expand Down
52 changes: 30 additions & 22 deletions pandar_driver/src/driver/pandar_driver.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -21,52 +21,60 @@ PandarDriver::PandarDriver(ros::NodeHandle node, ros::NodeHandle private_nh)

if (!pcap_path_.empty()) {
input_.reset(new PcapInput(lidar_port_, gps_port_, pcap_path_, model_));
} else {
}
else {
input_.reset(new SocketInput(lidar_port_, gps_port_));
}

if(model_ == "Pandar40P" || model_ == "Pandar40M"){
azimuth_index_ = 2; // 2 + 124 * [0-9]
is_valid_packet_ = [](size_t packet_size){ return (packet_size == 1262 || packet_size == 1266);};
}else if(model_ == "PandarQT"){
azimuth_index_ = 12; // 12 + 258 * [0-3]
is_valid_packet_ = [](size_t packet_size){ return (packet_size == 1072);};
}else if(model_ == "Pandar64"){
azimuth_index_ = 8; // 8 + 192 * [0-5]
is_valid_packet_ = [](size_t packet_size){ return (packet_size == 1194 || packet_size == 1198);};
}else if(model_ == "Pandar128"){
azimuth_index_ = 12; // 12 + 386 * [0-1]
is_valid_packet_ = [](size_t packet_size){ return (packet_size == 812);};
}else{
if (model_ == "Pandar40P" || model_ == "Pandar40M") {
azimuth_index_ = 2; // 2 + 124 * [0-9]
is_valid_packet_ = [](size_t packet_size) { return (packet_size == 1262 || packet_size == 1266); };
}
else if (model_ == "PandarQT") {
azimuth_index_ = 12; // 12 + 258 * [0-3]
is_valid_packet_ = [](size_t packet_size) { return (packet_size == 1072); };
}
else if (model_ == "Pandar64") {
azimuth_index_ = 8; // 8 + 192 * [0-5]
is_valid_packet_ = [](size_t packet_size) { return (packet_size == 1194 || packet_size == 1198); };
}
else if (model_ == "Pandar128") {
azimuth_index_ = 12; // 12 + 386 * [0-1]
is_valid_packet_ = [](size_t packet_size) { return (packet_size == 812); };
}
else {
ROS_ERROR("Invalid model name");
}
}

bool PandarDriver::poll(void){
bool PandarDriver::poll(void)
{
int scan_phase = static_cast<int>(scan_phase_ * 100.0);

pandar_msgs::PandarScanPtr scan(new pandar_msgs::PandarScan);
for(int prev_phase = 0;;){ // finish scan
while(true){ // until receive lidar packet
for (int prev_phase = 0;;) { // finish scan
while (true) { // until receive lidar packet
pandar_msgs::PandarPacket packet;
int packet_type = input_->getPacket(&packet);
if (packet_type == 0 && is_valid_packet_(packet.size)) {
scan->packets.push_back(packet);
break;
} else if(packet_type == -1){
}
else if (packet_type == -1) {
return false;
}
}

int current_phase = 0;
{
const auto& data = scan->packets.back().data;
current_phase = (data[azimuth_index_] & 0xff) | ((data[azimuth_index_+1] & 0xff) << 8);
current_phase = (static_cast<int>(current_phase) + 36000 - scan_phase) % 36000;
current_phase = (data[azimuth_index_] & 0xff) | ((data[azimuth_index_ + 1] & 0xff) << 8);
current_phase = (static_cast<int>(current_phase) + 36000 - scan_phase) % 36000;
}
if(current_phase >= prev_phase || scan->packets.size() < 2){
if (current_phase >= prev_phase || scan->packets.size() < 2) {
prev_phase = current_phase;
}else{
}
else {
// has scanned !
break;
}
Expand Down
62 changes: 25 additions & 37 deletions pandar_driver/src/lib/pcap_input.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -14,10 +14,8 @@ const uint8_t PKT_HEADER_SIZE = 42;
const size_t LIMIT_PACKET_NUM = 100;
} // namespace


PcapInput::PcapInput(uint16_t port, uint16_t gps_port, std::string path, std::string model):
pcap_(nullptr),
last_pkt_ts_(0)
PcapInput::PcapInput(uint16_t port, uint16_t gps_port, std::string path, std::string model)
: pcap_(nullptr), last_pkt_ts_(0)
{
initTimeIndexMap();
pcap_path_ = path;
Expand All @@ -26,8 +24,8 @@ last_pkt_ts_(0)
if (iter != time_index_map_.end()) {
ts_index_ = iter->second.first;
utc_index_ = iter->second.second;

} else {
}
else {
ts_index_ = 0;
utc_index_ = 0;
}
Expand All @@ -45,7 +43,7 @@ last_pkt_ts_(0)

std::stringstream filter;
filter << "udp dst port " << port;
if (pcap_compile(pcap_, &pcap_filter_, filter.str().c_str(), 1, PCAP_NETMASK_UNKNOWN) == -1){
if (pcap_compile(pcap_, &pcap_filter_, filter.str().c_str(), 1, PCAP_NETMASK_UNKNOWN) == -1) {
printf("compile pcap file fail\n");
return;
}
Expand All @@ -63,29 +61,20 @@ PcapInput::~PcapInput()

void PcapInput::initTimeIndexMap()
{
time_index_map_.insert(
std::pair<std::string, std::pair<int, int>>("Pandar40P", std::pair<int, int>(1250, 1256)));
time_index_map_.insert(
std::pair<std::string, std::pair<int, int>>("Pandar40M", std::pair<int, int>(1250, 1256)));
time_index_map_.insert(
std::pair<std::string, std::pair<int, int>>("Pandar64", std::pair<int, int>(1182, 1188)));
time_index_map_.insert(
std::pair<std::string, std::pair<int, int>>("PandarQT", std::pair<int, int>(1056, 1062)));
time_index_map_.insert(
std::pair<std::string, std::pair<int, int>>("Pandar20A", std::pair<int, int>(1258, 1264)));
time_index_map_.insert(
std::pair<std::string, std::pair<int, int>>("Pandar20B", std::pair<int, int>(1258, 1264)));
time_index_map_.insert(
std::pair<std::string, std::pair<int, int>>("PandarXT-32", std::pair<int, int>(1071, 1065)));
time_index_map_.insert(
std::pair<std::string, std::pair<int, int>>("PandarXT-16", std::pair<int, int>(559, 553)));
time_index_map_.insert(std::pair<std::string, std::pair<int, int>>("Pandar40P", std::pair<int, int>(1250, 1256)));
time_index_map_.insert(std::pair<std::string, std::pair<int, int>>("Pandar40M", std::pair<int, int>(1250, 1256)));
time_index_map_.insert(std::pair<std::string, std::pair<int, int>>("Pandar64", std::pair<int, int>(1182, 1188)));
time_index_map_.insert(std::pair<std::string, std::pair<int, int>>("PandarQT", std::pair<int, int>(1056, 1062)));
time_index_map_.insert(std::pair<std::string, std::pair<int, int>>("Pandar20A", std::pair<int, int>(1258, 1264)));
time_index_map_.insert(std::pair<std::string, std::pair<int, int>>("Pandar20B", std::pair<int, int>(1258, 1264)));
time_index_map_.insert(std::pair<std::string, std::pair<int, int>>("PandarXT-32", std::pair<int, int>(1071, 1065)));
time_index_map_.insert(std::pair<std::string, std::pair<int, int>>("PandarXT-16", std::pair<int, int>(559, 553)));
}


int PcapInput::getPacket(pandar_msgs::PandarPacket *pandar_pkt)
int PcapInput::getPacket(pandar_msgs::PandarPacket* pandar_pkt)
{
pcap_pkthdr * pkt_header;
const uint8_t * pkt_data;
pcap_pkthdr* pkt_header;
const uint8_t* pkt_data;
int64_t current_time;
int64_t pkt_ts = 0;

Expand All @@ -94,18 +83,17 @@ int PcapInput::getPacket(pandar_msgs::PandarPacket *pandar_pkt)
if (pcap_offline_filter(&pcap_filter_, pkt_header, pkt_data) == 0) {
continue;
}
const uint8_t * packet = pkt_data + PKT_HEADER_SIZE;
const uint8_t* packet = pkt_data + PKT_HEADER_SIZE;
int pkt_size = pkt_header->len - PKT_HEADER_SIZE;
pandar_pkt->stamp = ros::Time::now();
pandar_pkt->size = pkt_size;
std::memcpy(&pandar_pkt->data[0], packet, pkt_size);


packet_count_++;
// Sleep
if (packet_count_ >= LIMIT_PACKET_NUM && utc_index_ != 0) {
packet_count_ = 0;

struct tm t;
t.tm_year = packet[utc_index_];
t.tm_mon = packet[utc_index_ + 1] - 1;
Expand All @@ -115,17 +103,18 @@ int PcapInput::getPacket(pandar_msgs::PandarPacket *pandar_pkt)
t.tm_sec = packet[utc_index_ + 5];
t.tm_isdst = 0;

pkt_ts = mktime(&t) * 1000000 +
((packet[ts_index_] & 0xff) | (packet[ts_index_ + 1] & 0xff) << 8 |
((packet[ts_index_ + 2] & 0xff) << 16) | ((packet[ts_index_ + 3] & 0xff) << 24));
pkt_ts =
mktime(&t) * 1000000 + ((packet[ts_index_] & 0xff) | (packet[ts_index_ + 1] & 0xff) << 8 |
((packet[ts_index_ + 2] & 0xff) << 16) | ((packet[ts_index_ + 3] & 0xff) << 24));
struct timeval sys_time;
gettimeofday(&sys_time, nullptr);
current_time = sys_time.tv_sec * 1000000 + sys_time.tv_usec;

if (0 == last_pkt_ts_) {
last_pkt_ts_ = pkt_ts;
last_time_ = current_time;
} else {
}
else {
int64_t sleep_time = (pkt_ts - last_pkt_ts_) - (current_time - last_time_);
if (sleep_time > 0) {
struct timeval waitTime;
Expand All @@ -145,10 +134,9 @@ int PcapInput::getPacket(pandar_msgs::PandarPacket *pandar_pkt)
}
}
return 0;
}else{
}
else {
return -1;
}
}
}


40 changes: 22 additions & 18 deletions pandar_driver/src/lib/socket_input.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -31,11 +31,13 @@

using namespace pandar_driver;

namespace{
const size_t ETHERNET_MTU = 1500;
namespace
{
const size_t ETHERNET_MTU = 1500;
}

SocketInput::SocketInput(uint16_t port, uint16_t gpsPort) {
SocketInput::SocketInput(uint16_t port, uint16_t gpsPort)
{
// LOG_D("port: %d, gpsPort: %d", port,gpsPort);
socketForLidar = -1;
socketForLidar = socket(PF_INET, SOCK_DGRAM, 0);
Expand All @@ -50,8 +52,7 @@ SocketInput::SocketInput(uint16_t port, uint16_t gpsPort) {
myAddress.sin_port = htons(port); // port in network byte order
myAddress.sin_addr.s_addr = INADDR_ANY; // automatically fill in my IP

if (bind(socketForLidar, reinterpret_cast<sockaddr *>(&myAddress),
sizeof(sockaddr)) == -1) {
if (bind(socketForLidar, reinterpret_cast<sockaddr*>(&myAddress), sizeof(sockaddr)) == -1) {
perror("bind"); // TODO(Philip.Pi): perror errno
return;
}
Expand All @@ -77,10 +78,9 @@ SocketInput::SocketInput(uint16_t port, uint16_t gpsPort) {
memset(&myAddressGPS, 0, sizeof(myAddressGPS)); // initialize to zeros
myAddressGPS.sin_family = AF_INET; // host byte order
myAddressGPS.sin_port = htons(gpsPort); // port in network byte order
myAddressGPS.sin_addr.s_addr = INADDR_ANY; // automatically fill in my IP
myAddressGPS.sin_addr.s_addr = INADDR_ANY; // automatically fill in my IP

if (bind(socketForGPS, reinterpret_cast<sockaddr *>(&myAddressGPS),
sizeof(sockaddr)) == -1) {
if (bind(socketForGPS, reinterpret_cast<sockaddr*>(&myAddressGPS), sizeof(sockaddr)) == -1) {
perror("bind"); // TODO(Philip.Pi): perror errno
return;
}
Expand All @@ -92,23 +92,28 @@ SocketInput::SocketInput(uint16_t port, uint16_t gpsPort) {
socketNumber = 2;
}

SocketInput::~SocketInput(void) {
if (socketForGPS > 0) close(socketForGPS);
if (socketForLidar > 0) (void)close(socketForLidar);
SocketInput::~SocketInput(void)
{
if (socketForGPS > 0)
close(socketForGPS);
if (socketForLidar > 0)
(void)close(socketForLidar);
}

// return : 0 - lidar
// 1 - gps
// -1 - error
int SocketInput::getPacket(pandar_msgs::PandarPacket *pkt) {
int SocketInput::getPacket(pandar_msgs::PandarPacket* pkt)
{
struct pollfd fds[socketNumber];
if (socketNumber == 2) {
fds[0].fd = socketForGPS;
fds[0].events = POLLIN;

fds[1].fd = socketForLidar;
fds[1].events = POLLIN;
} else if (socketNumber == 1) {
}
else if (socketNumber == 1) {
fds[0].fd = socketForLidar;
fds[0].events = POLLIN;
}
Expand All @@ -118,14 +123,14 @@ int SocketInput::getPacket(pandar_msgs::PandarPacket *pkt) {
socklen_t senderAddressLen = sizeof(senderAddress);
int retval = poll(fds, socketNumber, POLL_TIMEOUT);
if (retval < 0) { // poll() error?
if (errno != EINTR) printf("poll() error: %s", strerror(errno));
if (errno != EINTR)
printf("poll() error: %s", strerror(errno));
return -1;
}
if (retval == 0) {
return -1;
}
if ((fds[0].revents & POLLERR) || (fds[0].revents & POLLHUP) ||
(fds[0].revents & POLLNVAL)) {
if ((fds[0].revents & POLLERR) || (fds[0].revents & POLLHUP) || (fds[0].revents & POLLNVAL)) {
// device error?
perror("poll() reports Pandar error");
return -1;
Expand All @@ -137,8 +142,7 @@ int SocketInput::getPacket(pandar_msgs::PandarPacket *pkt) {
// double time = getNowTimeSec();
for (int i = 0; i != socketNumber; ++i) {
if (fds[i].revents & POLLIN) {
nbytes = recvfrom(fds[i].fd, &pkt->data[0], ETHERNET_MTU, 0,
reinterpret_cast<sockaddr *>(&senderAddress),
nbytes = recvfrom(fds[i].fd, &pkt->data[0], ETHERNET_MTU, 0, reinterpret_cast<sockaddr*>(&senderAddress),
&senderAddressLen);
break;
}
Expand Down
Loading

0 comments on commit 7cdebc8

Please sign in to comment.