提交 e30bfb22 编写于 作者: S storypku 提交者: Chang Songhong

Build: fine grained build targets for hesai driver

上级 3245148c
......@@ -7,38 +7,122 @@ cc_binary(
name = "libhesai_driver_component.so",
linkshared = True,
linkstatic = False,
deps = [":driver"],
deps = [
":hesai_component",
":hesai_convert_component",
],
)
cc_library(
name = "driver",
srcs = [
"component.cc",
"driver.cc",
"hesai40_parser.cc",
"hesai64_parser.cc",
"parser.cc",
"tcp_cmd_client.cc",
"udp_input.cc",
],
hdrs = [
"const_var.h",
"driver.h",
"hesai_convert_component.h",
"hesai_driver_component.h",
"parser.h",
"tcp_cmd_client.h",
"type_defs.h",
"udp_input.h",
name = "hesai_convert_component",
srcs = ["hesai_convert_component.cc"],
hdrs = ["hesai_convert_component.h"],
copts = ['-DMODULE_NAME=\\"hesai\\"'],
deps = [
":driver",
":parser_factory",
"//cyber",
"//modules/drivers/hesai/proto:config_cc_proto",
],
)
cc_library(
name = "hesai_component",
srcs = ["hesai_component.cc"],
hdrs = ["hesai_component.h"],
copts = ['-DMODULE_NAME=\\"hesai\\"'],
deps = [
":driver",
":parser_factory",
"//cyber",
"//modules/drivers/hesai/proto:config_cc_proto",
],
)
cc_library(
name = "driver",
srcs = ["driver.cc"],
hdrs = ["driver.h"],
deps = [
":parser",
":type_defs",
":udp_input",
"//cyber",
],
)
cc_library(
name = "parser_factory",
srcs = ["parser_factory.cc"],
hdrs = ["parser_factory.h"],
deps = [
":hesai40_parser",
":hesai64_parser",
":parser",
"//cyber",
],
)
cc_library(
name = "hesai64_parser",
srcs = ["hesai64_parser.cc"],
hdrs = ["hesai64_parser.h"],
deps = [
":parser",
"//cyber",
],
)
cc_library(
name = "hesai40_parser",
srcs = ["hesai40_parser.cc"],
hdrs = ["hesai40_parser.h"],
deps = [
":parser",
"//cyber",
],
)
cc_library(
name = "parser",
srcs = ["parser.cc"],
hdrs = ["parser.h"],
deps = [
":tcp_cmd_client",
":type_defs",
"//cyber",
"//modules/common/util",
"//modules/drivers/hesai/proto:config_cc_proto",
"//modules/drivers/hesai/proto:hesai_cc_proto",
"//modules/drivers/proto:pointcloud_cc_proto",
],
)
cc_library(
name = "udp_input",
srcs = ["udp_input.cc"],
hdrs = ["udp_input.h"],
deps = [
":type_defs",
"//cyber",
],
)
cc_library(
name = "tcp_cmd_client",
srcs = ["tcp_cmd_client.cc"],
hdrs = ["tcp_cmd_client.h"],
deps = ["//cyber"],
)
cc_library(
name = "type_defs",
hdrs = ["type_defs.h"],
deps = [":const_var"],
)
cc_library(
name = "const_var",
hdrs = ["const_var.h"],
)
cpplint()
......@@ -29,36 +29,27 @@
#include <vector>
#include "cyber/cyber.h"
#include "modules/drivers/hesai/const_var.h"
#include "modules/drivers/hesai/driver.h"
#include "modules/drivers/hesai/parser.h"
#include "modules/drivers/hesai/proto/config.pb.h"
#include "modules/drivers/hesai/proto/hesai.pb.h"
#include "modules/drivers/hesai/type_defs.h"
#include "modules/drivers/hesai/udp_input.h"
namespace apollo {
namespace drivers {
namespace hesai {
using apollo::cyber::Node;
using apollo::cyber::Writer;
using apollo::drivers::hesai::HesaiScan;
class HesaiDriver {
public:
HesaiDriver(const std::shared_ptr<Node>& node, const Config& conf,
const std::shared_ptr<Parser>& parser)
HesaiDriver(const std::shared_ptr<::apollo::cyber::Node>& node,
const Config& conf, const std::shared_ptr<Parser>& parser)
: node_(node), conf_(conf), parser_(parser) {}
~HesaiDriver() { Stop(); }
bool Init();
private:
std::shared_ptr<Node> node_ = nullptr;
std::shared_ptr<::apollo::cyber::Node> node_ = nullptr;
Config conf_;
std::shared_ptr<Parser> parser_ = nullptr;
std::shared_ptr<Input> input_ = nullptr;
std::shared_ptr<Writer<HesaiScan>> scan_writer_ = nullptr;
std::shared_ptr<::apollo::cyber::Writer<HesaiScan>> scan_writer_ = nullptr;
std::mutex packet_mutex_;
std::condition_variable packet_condition_;
std::thread poll_thread_;
......
......@@ -14,12 +14,15 @@
* limitations under the License.
*****************************************************************************/
#include "modules/drivers/hesai/parser.h"
#include "modules/drivers/hesai/hesai40_parser.h"
namespace apollo {
namespace drivers {
namespace hesai {
using ::apollo::cyber::Node;
using apollo::drivers::PointXYZIT;
Hesai40Parser::Hesai40Parser(const std::shared_ptr<Node> &node,
const Config &conf)
: Parser(node, conf) {
......
/******************************************************************************
* Copyright 2020 The Apollo Authors. All Rights Reserved.
*
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
* http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*****************************************************************************/
#pragma once
#include "cyber/cyber.h"
#include "modules/drivers/hesai/parser.h"
namespace apollo {
namespace drivers {
namespace hesai {
class Hesai40Parser : public Parser {
public:
Hesai40Parser(const std::shared_ptr<::apollo::cyber::Node>& node,
const Config& conf);
~Hesai40Parser();
protected:
void ParseRawPacket(const uint8_t* buf, const int len, bool* is_end) override;
private:
void CalcPointXYZIT(Hesai40Packet* pkt, int blockid);
double block_offset_[BLOCKS_PER_PACKET];
double laser_offset_[LASER_COUNT];
};
} // namespace hesai
} // namespace drivers
} // namespace apollo
......@@ -14,12 +14,15 @@
* limitations under the License.
*****************************************************************************/
#include "modules/drivers/hesai/parser.h"
#include "modules/drivers/hesai/hesai64_parser.h"
namespace apollo {
namespace drivers {
namespace hesai {
using Node = ::apollo::cyber::Node;
using apollo::drivers::PointXYZIT;
Hesai64Parser::Hesai64Parser(const std::shared_ptr<Node> &node,
const Config &conf)
: Parser(node, conf) {
......
/******************************************************************************
* Copyright 2020 The Apollo Authors. All Rights Reserved.
*
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
* http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*****************************************************************************/
#pragma once
#include "cyber/cyber.h"
#include "modules/drivers/hesai/parser.h"
namespace apollo {
namespace drivers {
namespace hesai {
class Hesai64Parser : public Parser {
public:
Hesai64Parser(const std::shared_ptr<::apollo::cyber::Node>& node,
const Config& conf);
~Hesai64Parser();
protected:
void ParseRawPacket(const uint8_t* buf, const int len, bool* is_end) override;
private:
void CalcPointXYZIT(Hesai64Packet* pkt, int blockid, uint8_t chLaserNumber);
double block_offset_[BLOCKS_PER_PACKET_L64] = {0};
double laser_offset_[LASER_COUNT_L64] = {0};
};
} // namespace hesai
} // namespace drivers
} // namespace apollo
/******************************************************************************
* Copyright 2020 The Apollo Authors. All Rights Reserved.
*
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
* http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*****************************************************************************/
#include "modules/drivers/hesai/hesai_component.h"
namespace apollo {
namespace drivers {
namespace hesai {
bool HesaiComponent::Init() {
if (!GetProtoConfig(&conf_)) {
AERROR << "load config error, file:" << config_file_path_;
return false;
}
AINFO << "conf:" << conf_.DebugString();
Parser* parser = ParserFactory::CreateParser(node_, conf_);
if (parser == nullptr) {
AERROR << "create parser error";
return false;
}
parser_.reset(parser);
driver_.reset(new HesaiDriver(node_, conf_, parser_));
if (!driver_->Init()) {
AERROR << "driver init error";
return false;
}
return true;
}
} // namespace hesai
} // namespace drivers
} // namespace apollo
......@@ -14,51 +14,24 @@
* limitations under the License.
*****************************************************************************/
#ifndef LIDAR_HESAI_HESAI_ALL_COMPONENT_H_
#define LIDAR_HESAI_HESAI_ALL_COMPONENT_H_
#pragma once
#include <list>
#include <memory>
#include <string>
#include <thread>
#include "modules/drivers/hesai/proto/config.pb.h"
#include "cyber/cyber.h"
#include "modules/drivers/hesai/const_var.h"
#include "modules/drivers/hesai/driver.h"
#include "modules/drivers/hesai/parser.h"
#include "modules/drivers/hesai/proto/config.pb.h"
#include "modules/drivers/hesai/type_defs.h"
#include "modules/drivers/hesai/parser_factory.h"
namespace apollo {
namespace drivers {
namespace hesai {
using apollo::cyber::Component;
class HesaiComponent : public Component<> {
class HesaiComponent : public ::apollo::cyber::Component<> {
public:
~HesaiComponent() {}
bool Init() override {
if (!GetProtoConfig(&conf_)) {
AERROR << "load config error, file:" << config_file_path_;
return false;
}
AINFO << "conf:" << conf_.DebugString();
Parser* parser = ParserFactory::CreateParser(node_, conf_);
if (parser == nullptr) {
AERROR << "create parser error";
return false;
}
parser_.reset(parser);
driver_.reset(new HesaiDriver(node_, conf_, parser_));
if (!driver_->Init()) {
AERROR << "driver init error";
return false;
}
return true;
}
bool Init() override;
private:
std::shared_ptr<HesaiDriver> driver_;
......@@ -71,5 +44,3 @@ CYBER_REGISTER_COMPONENT(HesaiComponent)
} // namespace hesai
} // namespace drivers
} // namespace apollo
#endif
/******************************************************************************
* Copyright 2020 The Apollo Authors. All Rights Reserved.
*
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
* http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*****************************************************************************/
#include "modules/drivers/hesai/hesai_convert_component.h"
namespace apollo {
namespace drivers {
namespace hesai {
using apollo::cyber::Component;
bool HesaiConvertComponent::Init() {
if (!GetProtoConfig(&conf_)) {
AERROR << "load config error, file:" << config_file_path_;
return false;
}
AINFO << "conf:" << conf_.DebugString();
Parser* parser = ParserFactory::CreateParser(node_, conf_);
if (parser == nullptr) {
AERROR << "create parser error";
return false;
}
parser_.reset(parser);
if (!parser_->Init()) {
return false;
}
AINFO << "HesaiConvertComponent init success";
return true;
}
bool HesaiConvertComponent::Proc(const std::shared_ptr<HesaiScan>& scan) {
return parser_->Parse(scan);
}
} // namespace hesai
} // namespace drivers
} // namespace apollo
......@@ -14,56 +14,26 @@
* limitations under the License.
*****************************************************************************/
#ifndef LIDAR_HESAI_HESAI_CONVERT_COMPONENT_H_
#define LIDAR_HESAI_HESAI_CONVERT_COMPONENT_H_
#pragma once
#include <list>
#include <memory>
#include <string>
#include <thread>
#include "cyber/cyber.h"
#include "modules/drivers/hesai/const_var.h"
#include "modules/drivers/hesai/parser.h"
#include "modules/drivers/hesai/type_defs.h"
#include "modules/drivers/hesai/proto/config.pb.h"
#include "modules/drivers/hesai/proto/hesai.pb.h"
#include "cyber/cyber.h"
#include "modules/drivers/hesai/parser_factory.h"
namespace apollo {
namespace drivers {
namespace hesai {
using apollo::cyber::Component;
using apollo::drivers::hesai::HesaiScan;
class HesaiConvertComponent : public Component<HesaiScan> {
class HesaiConvertComponent : public ::apollo::cyber::Component<HesaiScan> {
public:
~HesaiConvertComponent() {}
bool Init() override {
if (!GetProtoConfig(&conf_)) {
AERROR << "load config error, file:" << config_file_path_;
return false;
}
virtual ~HesaiConvertComponent() = default;
bool Init() override;
AINFO << "conf:" << conf_.DebugString();
Parser* parser = ParserFactory::CreateParser(node_, conf_);
if (parser == nullptr) {
AERROR << "create parser error";
return false;
}
parser_.reset(parser);
if (!parser_->Init()) {
return false;
}
AINFO << "HesaiConvertComponent init success";
return true;
}
bool Proc(const std::shared_ptr<HesaiScan>& scan) override {
return parser_->Parse(scan);
}
bool Proc(const std::shared_ptr<HesaiScan>& scan) override;
private:
std::shared_ptr<Parser> parser_;
......@@ -75,5 +45,3 @@ CYBER_REGISTER_COMPONENT(HesaiConvertComponent)
} // namespace hesai
} // namespace drivers
} // namespace apollo
#endif
......@@ -24,7 +24,10 @@ namespace apollo {
namespace drivers {
namespace hesai {
Parser::Parser(const std::shared_ptr<Node>& node, const Config& conf)
using apollo::drivers::PointCloud;
Parser::Parser(const std::shared_ptr<::apollo::cyber::Node>& node,
const Config& conf)
: node_(node), conf_(conf) {
tz_second_ = conf_.time_zone() * 3600;
start_angle_ = static_cast<int>(conf_.start_angle() * 100);
......@@ -268,18 +271,6 @@ void Parser::CheckPktTime(double time_sec) {
}
}
Parser* ParserFactory::CreateParser(const std::shared_ptr<Node>& node,
const Config& conf) {
if (conf.model() == HESAI40P) {
return new Hesai40Parser(node, conf);
} else if (conf.model() == HESAI64) {
return new Hesai64Parser(node, conf);
}
AERROR << "only support HESAI40P | HESAI64";
return nullptr;
}
} // namespace hesai
} // namespace drivers
} // namespace apollo
......@@ -20,16 +20,16 @@
#include <deque>
#include <memory>
#include <string>
#include <thread>
#include <vector>
#include "cyber/base/concurrent_object_pool.h"
#include "cyber/cyber.h"
#include "modules/drivers/hesai/const_var.h"
#include "modules/drivers/hesai/proto/config.pb.h"
#include "modules/drivers/hesai/proto/hesai.pb.h"
#include "modules/drivers/proto/pointcloud.pb.h"
#include "cyber/cyber.h"
#include "modules/drivers/hesai/tcp_cmd_client.h"
#include "modules/drivers/hesai/type_defs.h"
#include "modules/drivers/proto/pointcloud.pb.h"
namespace apollo {
namespace drivers {
......@@ -37,17 +37,10 @@ namespace hesai {
inline double degreeToRadian(double degree) { return degree * PI / 180; }
using apollo::drivers::PointCloud;
using apollo::drivers::PointXYZIT;
using apollo::drivers::hesai::HesaiScan;
using apollo::cyber::Node;
using apollo::cyber::Writer;
using apollo::cyber::base::CCObjectPool;
class Parser {
public:
Parser(const std::shared_ptr<Node>& node, const Config& conf);
Parser(const std::shared_ptr<::apollo::cyber::Node>& node,
const Config& conf);
virtual ~Parser();
void Parse(const uint8_t* data, int size, bool* is_end);
bool Parse(const std::shared_ptr<HesaiScan>& scan);
......@@ -73,9 +66,9 @@ class Parser {
void ResetRawPointCloud();
bool is_calibration_ = false;
std::shared_ptr<Node> node_;
std::shared_ptr<::apollo::cyber::Node> node_;
Config conf_;
std::shared_ptr<Writer<PointCloud>> raw_pointcloud_writer_;
std::shared_ptr<::apollo::cyber::Writer<PointCloud>> raw_pointcloud_writer_;
int pool_size_ = 8;
int pool_index_ = 0;
uint64_t raw_last_time_ = 0;
......@@ -95,42 +88,6 @@ class Parser {
double horizatal_azimuth_offset_map_[LASER_COUNT_L64] = {0};
};
/***********************hesai40P***********************/
class Hesai40Parser : public Parser {
public:
Hesai40Parser(const std::shared_ptr<Node>& node, const Config& conf);
~Hesai40Parser();
protected:
void ParseRawPacket(const uint8_t* buf, const int len, bool* is_end) override;
private:
void CalcPointXYZIT(Hesai40Packet* pkt, int blockid);
double block_offset_[BLOCKS_PER_PACKET];
double laser_offset_[LASER_COUNT];
};
/***********************hesai64***********************/
class Hesai64Parser : public Parser {
public:
Hesai64Parser(const std::shared_ptr<Node>& node, const Config& conf);
~Hesai64Parser();
protected:
void ParseRawPacket(const uint8_t* buf, const int len, bool* is_end) override;
private:
void CalcPointXYZIT(Hesai64Packet* pkt, int blockid, uint8_t chLaserNumber);
double block_offset_[BLOCKS_PER_PACKET_L64] = {0};
double laser_offset_[LASER_COUNT_L64] = {0};
};
class ParserFactory {
public:
static Parser* CreateParser(const std::shared_ptr<Node>& node,
const Config& conf);
};
} // namespace hesai
} // namespace drivers
} // namespace apollo
......
/******************************************************************************
* Copyright 2020 The Apollo Authors. All Rights Reserved.
*
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
* http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*****************************************************************************/
#include "modules/drivers/hesai/parser_factory.h"
#include "modules/drivers/hesai/hesai40_parser.h"
#include "modules/drivers/hesai/hesai64_parser.h"
namespace apollo {
namespace drivers {
namespace hesai {
using ::apollo::cyber::Node;
Parser* ParserFactory::CreateParser(const std::shared_ptr<Node>& node,
const Config& conf) {
if (conf.model() == HESAI40P) {
return new Hesai40Parser(node, conf);
} else if (conf.model() == HESAI64) {
return new Hesai64Parser(node, conf);
}
AERROR << "only support HESAI40P | HESAI64";
return nullptr;
}
} // namespace hesai
} // namespace drivers
} // namespace apollo
......@@ -14,5 +14,21 @@
* limitations under the License.
*****************************************************************************/
#include "modules/drivers/hesai/hesai_convert_component.h"
#include "modules/drivers/hesai/hesai_driver_component.h"
#pragma once
#include "cyber/cyber.h"
#include "modules/drivers/hesai/parser.h"
namespace apollo {
namespace drivers {
namespace hesai {
class ParserFactory {
public:
static Parser* CreateParser(
const std::shared_ptr<::apollo::cyber::Node>& node, const Config& conf);
};
} // namespace hesai
} // namespace drivers
} // namespace apollo
......@@ -15,25 +15,22 @@
*****************************************************************************/
#include <arpa/inet.h>
#include <errno.h>
#include <fcntl.h>
#include <linux/sockios.h>
#include <net/if.h>
#include <netinet/in.h>
#include <pthread.h>
#include <setjmp.h>
#include <signal.h>
#include <stdarg.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <sys/ioctl.h>
#include <sys/ipc.h>
#include <sys/msg.h>
#include <sys/socket.h>
#include <sys/types.h>
#include <syslog.h>
#include <unistd.h>
#include <linux/sockios.h>
#include <cerrno>
#include <csignal>
#include <cstdio>
#include <cstring>
#include "cyber/cyber.h"
#include "modules/drivers/hesai/tcp_cmd_client.h"
......
......@@ -17,6 +17,7 @@
#ifndef LIDAR_HESAI_SRC_TYPE_DEFS_H_
#define LIDAR_HESAI_SRC_TYPE_DEFS_H_
#include <ctime>
#include "modules/drivers/hesai/const_var.h"
namespace apollo {
......
......@@ -14,18 +14,20 @@
* limitations under the License.
*****************************************************************************/
#include "modules/drivers/hesai/udp_input.h"
#include <arpa/inet.h>
#include <errno.h>
#include <fcntl.h>
#include <poll.h>
#include <string.h>
#include <sys/file.h>
#include <sys/socket.h>
#include <sys/time.h>
#include <iostream>
#include <cerrno>
#include <cstring>
#include <sstream>
#include "modules/drivers/hesai/udp_input.h"
#include "cyber/cyber.h"
namespace apollo {
namespace drivers {
......
......@@ -17,13 +17,7 @@
#ifndef LIDAR_HESAI_SRC_INPUT_H_
#define LIDAR_HESAI_SRC_INPUT_H_
#include <netinet/in.h>
#include <stdio.h>
#include <unistd.h>
#include <string>
#include "cyber/cyber.h"
#include "modules/drivers/hesai/const_var.h"
#include <cstdint>
#include "modules/drivers/hesai/type_defs.h"
namespace apollo {
......
Markdown is supported
0% .
You are about to add 0 people to the discussion. Proceed with caution.
先完成此消息的编辑!
想要评论请 注册