Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

Enabled config a nick name for multi lidar topics #119

Open
wants to merge 4 commits into
base: master
Choose a base branch
from
Open

Enabled config a nick name for multi lidar topics #119

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
4 commits
Select commit Hold shift + click to select a range
626c6a8
Enabled config a nick name for multi lidar topics
mmokhtar761 Apr 19, 2024
2602e06
Added the shut-down functionality
mmokhtar761 Apr 27, 2024
96335c3
Followed the NavINST standard topics naming
mmokhtar761 May 1, 2024
d7220dd
updated topics naming
mmokhtar761 May 8, 2024
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
62 changes: 62 additions & 0 deletions config/HAP_config _multi org.json
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -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
}
}
]
}

95 changes: 95 additions & 0 deletions config/HAP_config _multi.json
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -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
}
}
]
}

6 changes: 3 additions & 3 deletions config/HAP_config.json
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down
47 changes: 47 additions & 0 deletions launch_ROS1/rviz_HAP_multi.launch
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -0,0 +1,47 @@
<launch>

<!--user configure parameters for ros start-->
<arg name="lvx_file_path" default="livox_test.lvx"/>
<arg name="bd_list" default="100000000000000"/>
<arg name="xfer_format" default="0"/>
<arg name="multi_topic" default="1"/>
<arg name="data_src" default="0"/>
<arg name="publish_freq" default="10.0"/>
<arg name="output_type" default="0"/>
<arg name="rviz_enable" default="false"/>
<arg name="rosbag_enable" default="false"/>
<arg name="cmdline_arg" default="$(arg bd_list)"/>
<arg name="msg_frame_id" default="livox_frame"/>
<arg name="lidar_bag" default="true"/>
<arg name="imu_bag" default="true"/>
<!--user configure parameters for ros end-->

<param name="xfer_format" value="$(arg xfer_format)"/>
<param name="multi_topic" value="$(arg multi_topic)"/>
<param name="data_src" value="$(arg data_src)"/>
<param name="publish_freq" type="double" value="$(arg publish_freq)"/>
<param name="output_data_type" value="$(arg output_type)"/>
<param name="cmdline_str" type="string" value="$(arg bd_list)"/>
<param name="cmdline_file_path" type="string" value="$(arg lvx_file_path)"/>
<param name="user_config_path" type="string" value="$(find livox_ros_driver2)/config/HAP_config _multi.json"/>
<param name="frame_id" type="string" value="$(arg msg_frame_id)"/>
<param name="enable_lidar_bag" type="bool" value="$(arg lidar_bag)"/>
<param name="enable_imu_bag" type="bool" value="$(arg imu_bag)"/>

<node name="livox_lidar_publisher2" pkg="livox_ros_driver2"
type="livox_ros_driver2_node" required="true"
output="screen" args="$(arg cmdline_arg)"/>

<group if="$(arg rviz_enable)">
<node name="livox_rviz" pkg="rviz" type="rviz" respawn="true"
args="-d $(find livox_ros_driver2)/config/display_point_cloud_ROS1.rviz"/>
</group>
<group if="$(arg rosbag_enable)">
<node pkg="rosbag" type="record" name="record" output="screen"
args="-a"/>
</group>

<!-- <node pkg="rosparam" type="rosparam" name="param_dump" args="dump /livox_lidar_publisher2"/> -->


</launch>
1 change: 1 addition & 0 deletions src/comm/comm.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand Down
16 changes: 8 additions & 8 deletions src/lddc.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand Down Expand Up @@ -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;
Expand Down
7 changes: 6 additions & 1 deletion src/lds_lidar.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -133,7 +133,7 @@ bool LdsLidar::InitLivoxLidar() {

// parse user config
LivoxLidarConfigParser parser(path_);
std::vector<UserLivoxLidarConfig> user_configs;

if (!parser.Parse(user_configs)) {
std::cout << "failed to parse user-defined config" << std::endl;
}
Expand Down Expand Up @@ -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");
}
Expand Down
1 change: 1 addition & 0 deletions src/lds_lidar.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -54,6 +54,7 @@ class LdsLidar final : public Lds {

int DeInitLdsLidar(void);
private:
std::vector<UserLivoxLidarConfig> user_configs;
LdsLidar(double publish_freq);
LdsLidar(const LdsLidar &) = delete;
~LdsLidar();
Expand Down
5 changes: 5 additions & 0 deletions src/parse_cfg_file/parse_livox_lidar_cfg.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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 {
Expand Down