diff --git a/config/HAP_config _multi org.json b/config/HAP_config _multi org.json
new file mode 100644
index 00000000..6d3c46c4
--- /dev/null
+++ b/config/HAP_config _multi org.json
@@ -0,0 +1,62 @@
+{
+ "lidar_summary_info" : {
+ "lidar_type": 8
+ },
+ "HAP": {
+ "lidar_net_info" : {
+ "cmd_data_port": 56000,
+ "push_msg_port": 0,
+ "point_data_port": 57000,
+ "imu_data_port": 58000,
+ "log_data_port": 59000
+ },
+ "host_net_info" : {
+ "cmd_data_ip" : "192.168.1.2",
+ "cmd_data_port": 56000,
+ "push_msg_ip": "",
+ "push_msg_port": 0,
+ "point_data_ip": "192.168.1.2",
+ "point_data_port": 57000,
+ "imu_data_ip" : "192.168.1.2",
+ "imu_data_port": 58000,
+ "log_data_ip" : "",
+ "log_data_port": 59000
+ }
+ },
+
+
+ "lidar_configs" : [
+ {
+ "ip" : "192.168.1.101",
+ "name" : "R",
+ "pcl_data_type" : 1,
+ "pattern_mode" : 0,
+ "frame_id" : "rslidar",
+ "extrinsic_parameter" : {
+ "roll": 0.0,
+ "pitch": 0.0,
+ "yaw": 0.0,
+ "x": 0,
+ "y": 0,
+ "z": 0
+ }
+ }
+ ,
+ {
+ "ip" : "192.168.1.102",
+ "name" : "L",
+ "pcl_data_type" : 1,
+ "pattern_mode" : 0,
+ "frame_id" : "rslidar",
+ "extrinsic_parameter" : {
+ "roll": 0.0,
+ "pitch": 0.0,
+ "yaw": 0.0,
+ "x": 0,
+ "y": 0,
+ "z": 0
+ }
+ }
+ ]
+}
+
diff --git a/config/HAP_config _multi.json b/config/HAP_config _multi.json
new file mode 100644
index 00000000..ea8898f1
--- /dev/null
+++ b/config/HAP_config _multi.json
@@ -0,0 +1,95 @@
+{
+ "lidar_summary_info" : {
+ "lidar_type": 8
+ },
+ "HAP": {
+ "lidar_net_info" : {
+ "cmd_data_port": 56000,
+ "push_msg_port": 0,
+ "point_data_port": 57000,
+ "imu_data_port": 58000,
+ "log_data_port": 59000
+ },
+ "host_net_info" : {
+ "cmd_data_ip" : "192.168.1.2",
+ "cmd_data_port": 56000,
+ "push_msg_ip": "192.168.1.2",
+ "push_msg_port": 0,
+ "point_data_ip": "192.168.1.2",
+ "point_data_port": 57000,
+ "imu_data_ip" : "192.168.1.2",
+ "imu_data_port": 58000,
+ "log_data_ip" : "",
+ "log_data_port": 59000
+ }
+ },
+ "HAP": {
+ "lidar_net_info" : {
+ "cmd_data_port": 56000,
+ "push_msg_port": 0,
+ "point_data_port": 57000,
+ "imu_data_port": 58000,
+ "log_data_port": 59000
+ },
+ "host_net_info" : {
+ "cmd_data_ip" : "192.168.1.2",
+ "cmd_data_port": 56002,
+ "push_msg_ip": "192.168.1.2",
+ "push_msg_port": 2,
+ "point_data_ip": "192.168.1.2",
+ "point_data_port": 57002,
+ "imu_data_ip" : "192.168.1.2",
+ "imu_data_port": 58002,
+ "log_data_ip" : "",
+ "log_data_port": 59002
+ }
+ },
+
+ "lidar_configs" : [
+ {
+ "ip" : "192.168.1.101",
+ "name" : "Right",
+ "pcl_data_type" : 1,
+ "pattern_mode" : 0,
+ "extrinsic_parameter" : {
+ "roll": 0.0,
+ "pitch": 0.0,
+ "yaw": -120.0,
+ "x": 0,
+ "y": 0,
+ "z": 0
+ }
+ }
+ ,
+ {
+ "ip" : "192.168.1.102",
+ "name" : "Left",
+ "pcl_data_type" : 1,
+ "pattern_mode" : 0,
+ "extrinsic_parameter" : {
+ "roll": 0.0,
+ "pitch": 0.0,
+ "yaw": 120.0,
+ "x": 0,
+ "y": 0,
+ "z": 0
+ }
+ }
+ ,
+ {
+ "ip" : "192.168.1.100",
+ "name" : "Front",
+ "pcl_data_type" : 1,
+ "pattern_mode" : 0,
+ "extrinsic_parameter" : {
+ "roll": 0.0,
+ "pitch": 0.0,
+ "yaw": 0.0,
+ "x": 0,
+ "y": 0,
+ "z": 0
+ }
+ }
+ ]
+}
+
diff --git a/config/HAP_config.json b/config/HAP_config.json
index 090ab9f5..da764d0e 100644
--- a/config/HAP_config.json
+++ b/config/HAP_config.json
@@ -11,13 +11,13 @@
"log_data_port": 59000
},
"host_net_info" : {
- "cmd_data_ip" : "192.168.1.5",
+ "cmd_data_ip" : "192.168.1.2",
"cmd_data_port": 56000,
"push_msg_ip": "",
"push_msg_port": 0,
- "point_data_ip": "192.168.1.5",
+ "point_data_ip": "192.168.1.2",
"point_data_port": 57000,
- "imu_data_ip" : "192.168.1.5",
+ "imu_data_ip" : "192.168.1.2",
"imu_data_port": 58000,
"log_data_ip" : "",
"log_data_port": 59000
diff --git a/launch_ROS1/rviz_HAP_multi.launch b/launch_ROS1/rviz_HAP_multi.launch
new file mode 100644
index 00000000..e477992f
--- /dev/null
+++ b/launch_ROS1/rviz_HAP_multi.launch
@@ -0,0 +1,47 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/src/comm/comm.h b/src/comm/comm.h
index 492b39e1..681b20e0 100644
--- a/src/comm/comm.h
+++ b/src/comm/comm.h
@@ -265,6 +265,7 @@ typedef struct {
int32_t blind_spot_set;
int8_t dual_emit_en;
ExtParameter extrinsic_param;
+ std::string ldName;
volatile uint32_t set_bits;
volatile uint32_t get_bits;
} UserLivoxLidarConfig;
diff --git a/src/lddc.cpp b/src/lddc.cpp
index 06195df4..7181c7a4 100644
--- a/src/lddc.cpp
+++ b/src/lddc.cpp
@@ -571,13 +571,13 @@ PublisherPtr Lddc::GetCurrentPublisher(uint8_t index) {
char name_str[48];
memset(name_str, 0, sizeof(name_str));
if (use_multi_topic_) {
- std::string ip_string = IpNumToString(lds_->lidars_[index].handle);
- snprintf(name_str, sizeof(name_str), "livox/lidar_%s",
- ReplacePeriodByUnderline(ip_string).c_str());
+ std::string ldName = lds_->lidars_[index].livox_config.ldName;
+ snprintf(name_str, sizeof(name_str), "livox/lidar/%s/points",
+ ReplacePeriodByUnderline(ldName).c_str());
DRIVER_INFO(*cur_node_, "Support multi topics.");
} else {
DRIVER_INFO(*cur_node_, "Support only one topic.");
- snprintf(name_str, sizeof(name_str), "livox/lidar");
+ snprintf(name_str, sizeof(name_str), "livox/lidar/points");
}
*pub = new ros::Publisher;
@@ -622,12 +622,12 @@ PublisherPtr Lddc::GetCurrentImuPublisher(uint8_t handle) {
memset(name_str, 0, sizeof(name_str));
if (use_multi_topic_) {
DRIVER_INFO(*cur_node_, "Support multi topics.");
- std::string ip_string = IpNumToString(lds_->lidars_[handle].handle);
- snprintf(name_str, sizeof(name_str), "livox/imu_%s",
- ReplacePeriodByUnderline(ip_string).c_str());
+ std::string ldName = lds_->lidars_[handle].livox_config.ldName;
+ snprintf(name_str, sizeof(name_str), "livox/lidar/%s/imu",
+ ReplacePeriodByUnderline(ldName).c_str());
} else {
DRIVER_INFO(*cur_node_, "Support only one topic.");
- snprintf(name_str, sizeof(name_str), "livox/imu");
+ snprintf(name_str, sizeof(name_str), "livox/lidar/imu");
}
*pub = new ros::Publisher;
diff --git a/src/lds_lidar.cpp b/src/lds_lidar.cpp
index 6c739b08..a65f4854 100644
--- a/src/lds_lidar.cpp
+++ b/src/lds_lidar.cpp
@@ -133,7 +133,7 @@ bool LdsLidar::InitLivoxLidar() {
// parse user config
LivoxLidarConfigParser parser(path_);
- std::vector user_configs;
+
if (!parser.Parse(user_configs)) {
std::cout << "failed to parse user-defined config" << std::endl;
}
@@ -202,6 +202,11 @@ int LdsLidar::DeInitLdsLidar(void) {
}
if (lidar_summary_info_.lidar_type & kLivoxLidarType) {
+ // setting the lidars to idel state when closing
+ LivoxLidarWorkMode mode = kLivoxLidarWakeUp;
+ for (auto& config : user_configs) {
+ SetLivoxLidarWorkMode(config.handle, mode, nullptr , nullptr);
+ }
LivoxLidarSdkUninit();
printf("Livox Lidar SDK Deinit completely!\n");
}
diff --git a/src/lds_lidar.h b/src/lds_lidar.h
index b35c9c22..3c4f085c 100644
--- a/src/lds_lidar.h
+++ b/src/lds_lidar.h
@@ -54,6 +54,7 @@ class LdsLidar final : public Lds {
int DeInitLdsLidar(void);
private:
+ std::vector user_configs;
LdsLidar(double publish_freq);
LdsLidar(const LdsLidar &) = delete;
~LdsLidar();
diff --git a/src/parse_cfg_file/parse_livox_lidar_cfg.cpp b/src/parse_cfg_file/parse_livox_lidar_cfg.cpp
index 9360733c..52648a8c 100644
--- a/src/parse_cfg_file/parse_livox_lidar_cfg.cpp
+++ b/src/parse_cfg_file/parse_livox_lidar_cfg.cpp
@@ -72,6 +72,11 @@ bool LivoxLidarConfigParser::ParseUserConfigs(const rapidjson::Document &doc,
// parse user configs
user_config.handle = IpStringToNum(std::string(config["ip"].GetString()));
+ if(!config.HasMember("name")) {
+ user_config.ldName = config["ip"].GetString();
+ } else {
+ user_config.ldName = config["name"].GetString();
+ }
if (!config.HasMember("pcl_data_type")) {
user_config.pcl_data_type = -1;
} else {