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

Feature/robobus:Configure multiple lidar multiple frame_id in the configuration file，No need to install livox_SDK2 manually: #85

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

Feature/robobus:Configure multiple lidar multiple frame_id in the configuration file，No need to install livox_SDK2 manually: #85

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
5b37725
feat(livox_ros_driver2): ros2 humble环境编译通过
zymouse Oct 10, 2023
80fcc66
feat:Multiple lidars in different frames
Nov 6, 2023
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
1 change: 0 additions & 1 deletion .gitignore
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -1,4 +1,3 @@
.vscode
build
package.xml
__pycache__
12 changes: 10 additions & 2 deletions CMakeLists.txt
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -1,3 +1,8 @@
set(VERSION_ROS2 "ROS2" )
set(VERSION_HUMBLE "humble" )
set(ROS_EDITION ${VERSION_ROS2} )
set(HUMBLE_ROS ${ROS_HUMBLE} )
Comment on lines +1 to +4
Copy link

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

This part probably wasn't meant to be hardcoded - but otherwise it all seems to work well.


# judge which cmake codes to use
if(ROS_EDITION STREQUAL "ROS1")

Expand Down Expand Up @@ -245,12 +250,15 @@ else(ROS_EDITION STREQUAL "ROS2")
LIBRARY_NAME ${PROJECT_NAME}
)


## make sure the livox_lidar_sdk_shared library is installed
find_library(LIVOX_LIDAR_SDK_LIBRARY liblivox_lidar_sdk_shared.so /usr/local/lib REQUIRED)
# find_library(LIVOX_LIDAR_SDK_LIBRARY liblivox_lidar_sdk_shared.so /usr/local/lib REQUIRED)
find_library(LIVOX_LIDAR_SDK_LIBRARY liblivox_lidar_sdk_shared.so ${livox_sdk2_DIR}/../../../lib REQUIRED)

##
find_path(LIVOX_LIDAR_SDK_INCLUDE_DIR
NAMES "livox_lidar_api.h" "livox_lidar_def.h"
PATHS liblivox_lidar_sdk_shared.so ${livox_sdk2_DIR}/../../../include
REQUIRED)

## PCL library
Expand Down Expand Up @@ -297,7 +305,7 @@ else(ROS_EDITION STREQUAL "ROS2")
${PCL_INCLUDE_DIRS}
${APR_INCLUDE_DIRS}
${LIVOX_LIDAR_SDK_INCLUDE_DIR}
${LIVOX_INTERFACES_INCLUDE_DIRECTORIES} # for custom msgs
# ${LIVOX_INTERFACES_INCLUDE_DIRECTORIES} # for custom msgs
3rdparty
src
)
Expand Down
1 change: 1 addition & 0 deletions config/HAP_config.json
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -28,6 +28,7 @@
"ip" : "192.168.1.100",
"pcl_data_type" : 1,
"pattern_mode" : 0,
"frame_id" : "livox_had_frame",
"extrinsic_parameter" : {
"roll": 0.0,
"pitch": 0.0,
Expand Down
1 change: 1 addition & 0 deletions config/MID360_config.json
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -28,6 +28,7 @@
"ip" : "192.168.1.12",
"pcl_data_type" : 1,
"pattern_mode" : 0,
"frame_id" : "livox_mid360_frame",
"extrinsic_parameter" : {
"roll": 0.0,
"pitch": 0.0,
Expand Down
2 changes: 2 additions & 0 deletions package_ROS2.xml → package.xml
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -19,6 +19,8 @@
<depend>pcl_conversions</depend>
<depend>rcl_interfaces</depend>
<depend>libpcl-all-dev</depend>

<depend>livox_sdk2</depend>

<exec_depend>rosbag2</exec_depend>
<exec_depend>rosidl_default_runtime</exec_depend>
Expand Down
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 @@ -264,6 +264,7 @@ typedef struct {
int8_t pattern_mode;
int32_t blind_spot_set;
int8_t dual_emit_en;
std::string frame_id;
ExtParameter extrinsic_param;
volatile uint32_t set_bits;
volatile uint32_t get_bits;
Expand Down
46 changes: 24 additions & 22 deletions src/lddc.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -36,6 +36,7 @@

#include "driver_node.h"
#include "lds_lidar.h"
#include <string>

namespace livox_ros {

Expand Down Expand Up @@ -167,19 +168,20 @@ void Lddc::PollingLidarPointCloudData(uint8_t index, LidarDevice *lidar) {

while (!lds_->IsRequestExit() && !QueueIsEmpty(p_queue)) {
if (kPointCloud2Msg == transfer_format_) {
PublishPointcloud2(p_queue, index);
// PublishPointcloud2(p_queue, index);
PublishPointcloud2(p_queue, index, lidar->livox_config.frame_id);
} else if (kLivoxCustomMsg == transfer_format_) {
PublishCustomPointcloud(p_queue, index);
PublishCustomPointcloud(p_queue, index, lidar->livox_config.frame_id);
} else if (kPclPxyziMsg == transfer_format_) {
PublishPclMsg(p_queue, index);
PublishPclMsg(p_queue, index, lidar->livox_config.frame_id);
}
}
}

void Lddc::PollingLidarImuData(uint8_t index, LidarDevice *lidar) {
LidarImuDataQueue& p_queue = lidar->imu_data;
while (!lds_->IsRequestExit() && !p_queue.Empty()) {
PublishImuData(p_queue, index);
PublishImuData(p_queue, index, lidar->livox_config.frame_id);
}
}

Expand All @@ -198,7 +200,7 @@ void Lddc::PrepareExit(void) {
}
}

void Lddc::PublishPointcloud2(LidarDataQueue *queue, uint8_t index) {
void Lddc::PublishPointcloud2(LidarDataQueue *queue, uint8_t index, const std::string& frame_id) {
while(!QueueIsEmpty(queue)) {
StoragePacket pkg;
QueuePop(queue, &pkg);
Expand All @@ -209,12 +211,12 @@ void Lddc::PublishPointcloud2(LidarDataQueue *queue, uint8_t index) {

PointCloud2 cloud;
uint64_t timestamp = 0;
InitPointcloud2Msg(pkg, cloud, timestamp);
InitPointcloud2Msg(pkg, cloud, timestamp, frame_id);
PublishPointcloud2Data(index, timestamp, cloud);
}
}

void Lddc::PublishCustomPointcloud(LidarDataQueue *queue, uint8_t index) {
void Lddc::PublishCustomPointcloud(LidarDataQueue *queue, uint8_t index, const std::string& frame_id) {
while(!QueueIsEmpty(queue)) {
StoragePacket pkg;
QueuePop(queue, &pkg);
Expand All @@ -224,14 +226,14 @@ void Lddc::PublishCustomPointcloud(LidarDataQueue *queue, uint8_t index) {
}

CustomMsg livox_msg;
InitCustomMsg(livox_msg, pkg, index);
InitCustomMsg(livox_msg, pkg, index, frame_id);
FillPointsToCustomMsg(livox_msg, pkg);
PublishCustomPointData(livox_msg, index);
}
}

/* for pcl::pxyzi */
void Lddc::PublishPclMsg(LidarDataQueue *queue, uint8_t index) {
void Lddc::PublishPclMsg(LidarDataQueue *queue, uint8_t index, const std::string& frame_id) {
#ifdef BUILDING_ROS2
static bool first_log = true;
if (first_log) {
Expand All @@ -252,15 +254,15 @@ void Lddc::PublishPclMsg(LidarDataQueue *queue, uint8_t index) {

PointCloud cloud;
uint64_t timestamp = 0;
InitPclMsg(pkg, cloud, timestamp);
InitPclMsg(pkg, cloud, timestamp, frame_id);
FillPointsToPclMsg(pkg, cloud);
PublishPclData(index, timestamp, cloud);
}
return;
}

void Lddc::InitPointcloud2MsgHeader(PointCloud2& cloud) {
cloud.header.frame_id.assign(frame_id_);
void Lddc::InitPointcloud2MsgHeader(PointCloud2& cloud, const std::string& frame_id) {
cloud.header.frame_id.assign(frame_id);
cloud.height = 1;
cloud.width = 0;
cloud.fields.resize(7);
Expand Down Expand Up @@ -295,8 +297,8 @@ void Lddc::InitPointcloud2MsgHeader(PointCloud2& cloud) {
cloud.point_step = sizeof(LivoxPointXyzrtlt);
}

void Lddc::InitPointcloud2Msg(const StoragePacket& pkg, PointCloud2& cloud, uint64_t& timestamp) {
InitPointcloud2MsgHeader(cloud);
void Lddc::InitPointcloud2Msg(const StoragePacket& pkg, PointCloud2& cloud, uint64_t& timestamp, const std::string& frame_id) {
InitPointcloud2MsgHeader(cloud, frame_id);

cloud.point_step = sizeof(LivoxPointXyzrtlt);

Expand Down Expand Up @@ -351,8 +353,8 @@ void Lddc::PublishPointcloud2Data(const uint8_t index, const uint64_t timestamp,
}
}

void Lddc::InitCustomMsg(CustomMsg& livox_msg, const StoragePacket& pkg, uint8_t index) {
livox_msg.header.frame_id.assign(frame_id_);
void Lddc::InitCustomMsg(CustomMsg& livox_msg, const StoragePacket& pkg, uint8_t index, const std::string& frame_id) {
livox_msg.header.frame_id.assign(frame_id);

#ifdef BUILDING_ROS1
static uint32_t msg_seq = 0;
Expand Down Expand Up @@ -416,9 +418,9 @@ void Lddc::PublishCustomPointData(const CustomMsg& livox_msg, const uint8_t inde
}
}

void Lddc::InitPclMsg(const StoragePacket& pkg, PointCloud& cloud, uint64_t& timestamp) {
void Lddc::InitPclMsg(const StoragePacket& pkg, PointCloud& cloud, uint64_t& timestamp, const std::string& frame_id) {
#ifdef BUILDING_ROS1
cloud.header.frame_id.assign(frame_id_);
cloud.header.frame_id.assign(frame_id);
cloud.height = 1;
cloud.width = pkg.points_num;

Expand Down Expand Up @@ -477,8 +479,8 @@ void Lddc::PublishPclData(const uint8_t index, const uint64_t timestamp, const P
return;
}

void Lddc::InitImuMsg(const ImuData& imu_data, ImuMsg& imu_msg, uint64_t& timestamp) {
imu_msg.header.frame_id = "livox_frame";
void Lddc::InitImuMsg(const ImuData& imu_data, ImuMsg& imu_msg, uint64_t& timestamp, const std::string& frame_id) {
imu_msg.header.frame_id = frame_id;

timestamp = imu_data.time_stamp;
#ifdef BUILDING_ROS1
Expand All @@ -495,7 +497,7 @@ void Lddc::InitImuMsg(const ImuData& imu_data, ImuMsg& imu_msg, uint64_t& timest
imu_msg.linear_acceleration.z = imu_data.acc_z;
}

void Lddc::PublishImuData(LidarImuDataQueue& imu_data_queue, const uint8_t index) {
void Lddc::PublishImuData(LidarImuDataQueue& imu_data_queue, const uint8_t index, const std::string& frame_id) {
ImuData imu_data;
if (!imu_data_queue.Pop(imu_data)) {
//printf("Publish imu data failed, imu data queue pop failed.\n");
Expand All @@ -504,7 +506,7 @@ void Lddc::PublishImuData(LidarImuDataQueue& imu_data_queue, const uint8_t index

ImuMsg imu_msg;
uint64_t timestamp;
InitImuMsg(imu_data, imu_msg, timestamp);
InitImuMsg(imu_data, imu_msg, timestamp, frame_id);

#ifdef BUILDING_ROS1
PublisherPtr publisher_ptr = GetCurrentImuPublisher(index);
Expand Down
19 changes: 10 additions & 9 deletions src/lddc.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -29,6 +29,7 @@

#include "driver_node.h"
#include "lds.h"
#include <string>

namespace livox_ros {

Expand Down Expand Up @@ -100,25 +101,25 @@ class Lddc final {
void PollingLidarPointCloudData(uint8_t index, LidarDevice *lidar);
void PollingLidarImuData(uint8_t index, LidarDevice *lidar);

void PublishPointcloud2(LidarDataQueue *queue, uint8_t index);
void PublishCustomPointcloud(LidarDataQueue *queue, uint8_t index);
void PublishPclMsg(LidarDataQueue *queue, uint8_t index);
void PublishPointcloud2(LidarDataQueue *queue, uint8_t index, const std::string& frame_id);
void PublishCustomPointcloud(LidarDataQueue *queue, uint8_t index, const std::string& frame_id);
void PublishPclMsg(LidarDataQueue *queue, uint8_t index, const std::string& frame_id);

void PublishImuData(LidarImuDataQueue& imu_data_queue, const uint8_t index);
void PublishImuData(LidarImuDataQueue& imu_data_queue, const uint8_t index, const std::string& frame_id);

void InitPointcloud2MsgHeader(PointCloud2& cloud);
void InitPointcloud2Msg(const StoragePacket& pkg, PointCloud2& cloud, uint64_t& timestamp);
void InitPointcloud2MsgHeader(PointCloud2& cloud, const std::string& frame_id);
void InitPointcloud2Msg(const StoragePacket& pkg, PointCloud2& cloud, uint64_t& timestamp, const std::string& frame_id);
void PublishPointcloud2Data(const uint8_t index, uint64_t timestamp, const PointCloud2& cloud);

void InitCustomMsg(CustomMsg& livox_msg, const StoragePacket& pkg, uint8_t index);
void InitCustomMsg(CustomMsg& livox_msg, const StoragePacket& pkg, uint8_t index, const std::string& frame_id);
void FillPointsToCustomMsg(CustomMsg& livox_msg, const StoragePacket& pkg);
void PublishCustomPointData(const CustomMsg& livox_msg, const uint8_t index);

void InitPclMsg(const StoragePacket& pkg, PointCloud& cloud, uint64_t& timestamp);
void InitPclMsg(const StoragePacket& pkg, PointCloud& cloud, uint64_t& timestamp, const std::string& frame_id);
void FillPointsToPclMsg(const StoragePacket& pkg, PointCloud& pcl_msg);
void PublishPclData(const uint8_t index, const uint64_t timestamp, const PointCloud& cloud);

void InitImuMsg(const ImuData& imu_data, ImuMsg& imu_msg, uint64_t& timestamp);
void InitImuMsg(const ImuData& imu_data, ImuMsg& imu_msg, uint64_t& timestamp, const std::string& frame_id);

void FillPointsToPclMsg(PointCloud& pcl_msg, LivoxPointXyzrtlt* src_point, uint32_t num);
void FillPointsToCustomMsg(CustomMsg& livox_msg, LivoxPointXyzrtlt* src_point, uint32_t num,
Expand Down
1 change: 1 addition & 0 deletions src/parse_cfg_file/parse_cfg_file.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -27,6 +27,7 @@
#include <iostream>
#include <cstdio>
#include <arpa/inet.h>
#include <string>

namespace livox_ros {

Expand Down
7 changes: 7 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 @@ -92,6 +92,13 @@ bool LivoxLidarConfigParser::ParseUserConfigs(const rapidjson::Document &doc,
} else {
user_config.dual_emit_en = static_cast<uint8_t>(config["dual_emit_en"].GetInt());
}
if (!config.HasMember("frame_id")) {
user_config.frame_id = "livox_lidar";
std::cout << "No frame id was given, set to default of 'livox_lidar'" << std::endl;
} else {
user_config.frame_id = static_cast<std::string>(config["frame_id"].GetString());
}

if (!config.HasMember("extrinsic_parameter")) {
memset(&user_config.extrinsic_param, 0, sizeof(user_config.extrinsic_param));
} else {
Expand Down