Skip to content

Commit

Permalink
feat: add transformation parameter
Browse files Browse the repository at this point in the history
  • Loading branch information
@CrazyTomX
CrazyTomX committed Nov 6, 2020
1 parent bd84ce2 commit aff2e39
Show file tree
Hide file tree
Showing 8 changed files with 128 additions and 60 deletions.
3 changes: 1 addition & 2 deletions CHANGELOG.md
View file
Original file line number Diff line number Diff line change
Expand Up @@ -10,12 +10,11 @@
- Add rs_driver_viewer, a small tool to show point cloud
- Add save_by_rows argument
- Add multi-cast support

- Add points transformation function
### Changed

- Update some decoding part for LiDARS
- Change the definition of packet message

- Update documents


Expand Down
4 changes: 2 additions & 2 deletions CMakeLists.txt
View file
Original file line number Diff line number Diff line change
@@ -1,7 +1,7 @@
cmake_minimum_required(VERSION 3.5)
cmake_policy(SET CMP0048 NEW)
if(WIN32)
cmake_policy(SET CMP0074 NEW)
cmake_policy(SET CMP0074 NEW)
endif(WIN32)
project(rs_driver VERSION 1.3.0)

Expand Down Expand Up @@ -113,7 +113,7 @@ endif(WIN32)
# Eigen
#========================
if(${ENABLE_TRANSFORM})
add_definitions("-DENABLE_TRANSFORM" PARENT_SCOPE)
add_definitions("-DENABLE_TRANSFORM")
find_package(Eigen3 REQUIRED)
include_directories(${EIGEN3_INCLUDE_DIR})
message(=============================================================)
Expand Down
4 changes: 4 additions & 0 deletions cmake/rs_driverConfig.cmake.in
View file
Original file line number Diff line number Diff line change
Expand Up @@ -22,6 +22,10 @@ if(OPENMP_FOUND)
set(CMAKE_EXE_LINKER_FLAGS"${CMAKE_EXE_LINKER_FLAGS}${OpenMP_EXE_LINKER_FLAGS}")
endif()

if(${ENABLE_TRANSFORM})
add_definitions("-DENABLE_TRANSFORM")
endif(${ENABLE_TRANSFORM})

set(rs_driver_INCLUDE_DIRS "@DRIVER_INCLUDE_DIRS@;@INSTALL_DRIVER_DIR@")
set(RS_DRIVER_INCLUDE_DIRS "@DRIVER_INCLUDE_DIRS@;@INSTALL_DRIVER_DIR@")

Expand Down
2 changes: 1 addition & 1 deletion demo/demo_online.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -51,7 +51,7 @@ void exceptionCallback(const Error& code)
{
/* Note: Please do not put time-consuming operations in the callback function! */
/* Make a copy of the error message and process it in another thread is recommended*/
RS_WARNING << "Error code : " << code.toString() << RS_REND;
RS_WARNING << code.toString() << RS_REND;
}

int main(int argc, char* argv[])
Expand Down
8 changes: 4 additions & 4 deletions demo/demo_pcap.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -51,7 +51,7 @@ void exceptionCallback(const Error& code)
{
/* Note: Please do not put time-consuming operations in the callback function! */
/* Make a copy of the error message and process it in another thread is recommended*/
RS_WARNING << "Error code : " << code.toString() << RS_REND;
RS_WARNING << code.toString() << RS_REND;
}

int main(int argc, char* argv[])
Expand All @@ -66,9 +66,9 @@ int main(int argc, char* argv[])
RSDriverParam param; ///< Create a parameter object
param.input_param.read_pcap = true; ///< Set read_pcap to true
param.input_param.pcap_path = "/home/robosense/lidar.pcap"; ///< Set the pcap file directory
param.input_param.msop_port = 6699; ///< Set the lidar msop port number, the default is 6699
param.input_param.difop_port = 7788; ///< Set the lidar difop port number, the default is 7788
param.lidar_type = LidarType::RS16; ///< Set the lidar type. Make sure this type is correct
param.input_param.msop_port = 6699; ///< Set the lidar msop port number, the default is 6699
param.input_param.difop_port = 7788; ///< Set the lidar difop port number, the default is 7788
param.lidar_type = LidarType::RS16; ///< Set the lidar type. Make sure this type is correct
param.print();

driver.regExceptionCallback(exceptionCallback); ///< Register the exception callback function into the driver
Expand Down
10 changes: 5 additions & 5 deletions src/rs_driver/driver/decoder/decoder_base.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -712,11 +712,11 @@ template <typename T_Point>
inline void DecoderBase<T_Point>::transformPoint(float& x, float& y, float& z)
{
#ifdef ENABLE_TRANSFORM
RS_INFO<<"BBB"<<RS_REND;
Eigen::AngleAxisd current_rotation_x(0, Eigen::Vector3d::UnitX());
Eigen::AngleAxisd current_rotation_y(1.57, Eigen::Vector3d::UnitY());
Eigen::AngleAxisd current_rotation_z(0, Eigen::Vector3d::UnitZ());
Eigen::Translation3d current_translation(0, 0, 0);
Eigen::AngleAxisd current_rotation_x(param_.transform_param.roll, Eigen::Vector3d::UnitX());
Eigen::AngleAxisd current_rotation_y(param_.transform_param.pitch, Eigen::Vector3d::UnitY());
Eigen::AngleAxisd current_rotation_z(param_.transform_param.yaw, Eigen::Vector3d::UnitZ());
Eigen::Translation3d current_translation(param_.transform_param.x, param_.transform_param.y,
param_.transform_param.z);
Eigen::Matrix4d trans = (current_translation * current_rotation_z * current_rotation_y * current_rotation_x).matrix();
Eigen::Vector4d target_ori(x, y, z, 1);
Eigen::Vector4d target_rotate = trans * target_ori;
Expand Down
91 changes: 57 additions & 34 deletions src/rs_driver/driver/driver_param.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -26,7 +26,7 @@ namespace robosense
{
namespace lidar
{
enum LidarType ///< The lidar type
enum LidarType ///< LiDAR type
{
RS16 = 1,
RS32,
Expand All @@ -44,11 +44,12 @@ enum SplitFrameMode
SPLIT_BY_CUSTOM_PKTS
};

typedef struct RSCameraTriggerParam ///< The Camera trigger parameters
typedef struct RSCameraTriggerParam ///< Camera trigger parameters
{
std::map<double, std::string> trigger_map; ///< The map stored the trigger angle and camera frame id
void print() const ///< This function is used to print all the parameters for debugging
std::map<double, std::string> trigger_map; ///< Map stored the trigger angle and camera frame id
void print() const
{
RS_INFO << "------------------------------------------------------" << RS_REND;
RS_INFO << " RoboSense Camera Trigger Parameters " << RS_REND;
for (auto iter : trigger_map)
{
Expand All @@ -58,22 +59,45 @@ typedef struct RSCameraTriggerParam ///< The Camera trigger parameters
}
} RSCameraTriggerParam;

typedef struct RSDecoderParam ///< The lidar decoder parameter
typedef struct RSTransformParam ///< The Point transform parameter
{
float x = 0.0f; ///< unit, m
float y = 0.0f; ///< unit, m
float z = 0.0f; ///< unit, m
float roll = 0.0f; ///< unit, radian
float pitch = 0.0f; ///< unit, radian
float yaw = 0.0f; ///< unit, radian
void print() const
{
RS_INFO << "------------------------------------------------------" << RS_REND;
RS_INFO << " RoboSense Transform Parameters " << RS_REND;
RS_INFOL << "x: " << x << RS_REND;
RS_INFOL << "y: " << y << RS_REND;
RS_INFOL << "z: " << z << RS_REND;
RS_INFOL << "roll: " << roll << RS_REND;
RS_INFOL << "pitch: " << pitch << RS_REND;
RS_INFOL << "yaw: " << yaw << RS_REND;
RS_INFO << "------------------------------------------------------" << RS_REND;
}
} RSTransformParam;

typedef struct RSDecoderParam ///< LiDAR decoder parameter
{
float max_distance = 200.0f; ///< The max distance of point cloud range
float min_distance = 0.2f; ///< The minimum distance of point cloud range
float start_angle = 0.0f; ///< The start angle of point cloud
float end_angle = 360.0f; ///< The end angle of point cloud
SplitFrameMode split_frame_mode =
SplitFrameMode::SPLIT_BY_ANGLE; ///< 1: Split frames by cut_angle; 2: Split frames by fixed number of packets;
///< 3: Split frames by custom number of packets (num_pkts_split)
uint32_t num_pkts_split = 1; ///< The number of packets in one frame, only be used when split_frame_mode=3
float cut_angle = 0.0f; ///< The cut angle(degree) used to split frame, only be used when split_frame_mode=1
bool use_lidar_clock = false; ///< true: lidar message timestamp is the lidar clock
///< false: timestamp is the computer system clock
RSCameraTriggerParam trigger_param; ///< The parameter used to trigger camera
void print() const ///< This function is used to print all the parameters for debugging
float max_distance = 200.0f; ///< Max distance of point cloud range
float min_distance = 0.2f; ///< Minimum distance of point cloud range
float start_angle = 0.0f; ///< Start angle of point cloud
float end_angle = 360.0f; ///< End angle of point cloud
SplitFrameMode split_frame_mode = SplitFrameMode::SPLIT_BY_ANGLE; ///< 1: Split frames by cut_angle;
///< 2: Split frames by fixed number of packets;
///< 3: Split frames by custom number of packets (num_pkts_split)
uint32_t num_pkts_split = 1; ///< Number of packets in one frame, only be used when split_frame_mode=3
float cut_angle = 0.0f; ///< Cut angle(degree) used to split frame, only be used when split_frame_mode=1
bool use_lidar_clock = false; ///< true: use LiDAR clock as timestamp; false: use system clock as timestamp
RSTransformParam transform_param; ///< Used to transform points
RSCameraTriggerParam trigger_param; ///< Used to trigger camera
void print() const
{
transform_param.print();
trigger_param.print();
RS_INFO << "------------------------------------------------------" << RS_REND;
RS_INFO << " RoboSense Decoder Parameters " << RS_REND;
Expand All @@ -89,18 +113,18 @@ typedef struct RSDecoderParam ///< The lidar decoder parameter
}
} RSDecoderParam;

typedef struct RSInputParam ///< The lidar input parameter
typedef struct RSInputParam ///< The LiDAR input parameter
{
std::string device_ip = "192.168.1.200"; ///< The ip of lidar
std::string multi_cast_address = "0.0.0.0"; ///< The address of multicast
uint16_t msop_port = 6699; ///< The msop packet port number
uint16_t difop_port = 7788; ///< The difop packet port number
std::string device_ip = "192.168.1.200"; ///< Ip of LiDAR
std::string multi_cast_address = "0.0.0.0"; ///< Address of multicast
uint16_t msop_port = 6699; ///< Msop packet port number
uint16_t difop_port = 7788; ///< Difop packet port number
bool read_pcap = false; ///< true: The driver will process the pcap through pcap_path. false: The driver will
///< get data from online lidar
double pcap_rate = 1; ///< The rate to read the pcap file
///< Get data from online LiDAR
double pcap_rate = 1; ///< Rate to read the pcap file
bool pcap_repeat = true; ///< true: The pcap bag will repeat play
std::string pcap_path = "null"; ///< The absolute path of pcap file
void print() const ///< This function is used to print all the parameters for debugging
std::string pcap_path = "null"; ///< Absolute path of pcap file
void print() const
{
RS_INFO << "------------------------------------------------------" << RS_REND;
RS_INFO << " RoboSense Input Parameters " << RS_REND;
Expand All @@ -114,17 +138,16 @@ typedef struct RSInputParam ///< The lidar input parameter
}
} RSInputParam;

typedef struct RSDriverParam ///< The lidar driver parameter
typedef struct RSDriverParam ///< The LiDAR driver parameter
{
RSInputParam input_param; ///< The input parameter
RSDecoderParam decoder_param; ///< The decoder parameter
std::string angle_path = "null"; ///< The path of angle calibration files(angle.csv)
///< For the latest version lidar, this file is not needed
std::string frame_id = "rslidar"; ///< The frame id of lidar message
RSInputParam input_param; ///< Input parameter
RSDecoderParam decoder_param; ///< Decoder parameter
std::string angle_path = "null"; ///< Path of angle calibration files(angle.csv).Only used for internal debugging.
std::string frame_id = "rslidar"; ///< The frame id of LiDAR message
LidarType lidar_type = LidarType::RS16; ///< Lidar type
bool wait_for_difop = true; ///< true: start sending point cloud until receive difop packet
bool saved_by_rows = false; ///< true: the output point cloud will be saved by rows (default is saved by columns)
void print() const ///< This function is used to print all the parameters for debugging
void print() const
{
input_param.print();
decoder_param.print();
Expand Down
66 changes: 54 additions & 12 deletions tool/rs_driver_viewer.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -60,21 +60,43 @@ bool parseArgument(int argc, const char* const* argv, const char* str, std::stri

void parseParam(int argc, char* argv[], RSDriverParam& param)
{
param.wait_for_difop=false;
std::string lidar_type;
std::string msop_port;
std::string difop_port;
if (parseArgument(argc, argv, "-type", lidar_type))
param.wait_for_difop = false;
std::string result_str;
if (parseArgument(argc, argv, "-type", result_str))
{
param.lidar_type = param.strToLidarType(lidar_type);
param.lidar_type = param.strToLidarType(result_str);
}
if (parseArgument(argc, argv, "-msop", msop_port))
if (parseArgument(argc, argv, "-msop", result_str))
{
param.input_param.msop_port = std::stoi(msop_port);
param.input_param.msop_port = std::stoi(result_str);
}
if (parseArgument(argc, argv, "-difop", difop_port))
if (parseArgument(argc, argv, "-difop", result_str))
{
param.input_param.difop_port = std::stoi(difop_port);
param.input_param.difop_port = std::stoi(result_str);
}
if (parseArgument(argc, argv, "-x", result_str))
{
param.decoder_param.transform_param.x = std::stof(result_str);
}
if (parseArgument(argc, argv, "-y", result_str))
{
param.decoder_param.transform_param.y = std::stof(result_str);
}
if (parseArgument(argc, argv, "-z", result_str))
{
param.decoder_param.transform_param.z = std::stof(result_str);
}
if (parseArgument(argc, argv, "-roll", result_str))
{
param.decoder_param.transform_param.roll = std::stof(result_str);
}
if (parseArgument(argc, argv, "-pitch", result_str))
{
param.decoder_param.transform_param.pitch = std::stof(result_str);
}
if (parseArgument(argc, argv, "-yaw", result_str))
{
param.decoder_param.transform_param.yaw = std::stof(result_str);
}
if (parseArgument(argc, argv, "-pcap", param.input_param.pcap_path))
{
Expand All @@ -87,8 +109,15 @@ void printHelpMenu()
RS_MSG << "Arguments are: " << RS_REND;
RS_MSG << " -msop = LiDAR msop port number,the default value is 6699" << RS_REND;
RS_MSG << " -difop = LiDAR difop port number,the default value is 7788" << RS_REND;
RS_MSG << " -type = LiDAR type( RS16, RS32, RSBP, RS128, RS80, RSM1, RSHELIOS ), the default value is RS16"
RS_MSG << " -type = LiDAR type( RS16, RS32, RSBP, RS128, RS80, RSM1, RSHELIOS ), the default "
"value is RS16"
<< RS_REND;
RS_MSG << " -x = Transformation parameter, unit: m " << RS_REND;
RS_MSG << " -y = Transformation parameter, unit: m " << RS_REND;
RS_MSG << " -z = Transformation parameter, unit: m " << RS_REND;
RS_MSG << " -roll = Transformation parameter, unit: radian " << RS_REND;
RS_MSG << " -pitch = Transformation parameter, unit: radian " << RS_REND;
RS_MSG << " -yaw = Transformation parameter, unit: radian " << RS_REND;
RS_MSG << " -pcap = The path of the pcap file, if this argument is set, the driver "
"will work in off-line mode and read the pcap file. Otherwise the driver work in online mode."
<< RS_REND;
Expand All @@ -114,6 +143,19 @@ void printParam(const RSDriverParam& param)
RS_INFO << param.input_param.difop_port << RS_REND;
RS_INFOL << "LiDAR Type: ";
RS_INFO << param.lidarTypeToStr(param.lidar_type) << RS_REND;
RS_INFOL << "Transformation Parameters (x, y, z, roll, pitch, yaw): " << RS_REND;
RS_INFOL << "x: ";
RS_INFO << std::fixed << param.decoder_param.transform_param.x << RS_REND;
RS_INFOL << "y: ";
RS_INFO << std::fixed << param.decoder_param.transform_param.y << RS_REND;
RS_INFOL << "z: ";
RS_INFO << std::fixed << param.decoder_param.transform_param.z << RS_REND;
RS_INFOL << "roll: ";
RS_INFO << std::fixed << param.decoder_param.transform_param.yaw << RS_REND;
RS_INFOL << "pitch: ";
RS_INFO << std::fixed << param.decoder_param.transform_param.pitch << RS_REND;
RS_INFOL << "yaw: ";
RS_INFO << std::fixed << param.decoder_param.transform_param.yaw << RS_REND;
}
/**
* @brief The point cloud callback function. This function will be registered to lidar driver.
Expand Down Expand Up @@ -144,7 +186,7 @@ void exceptionCallback(const Error& code)
{
/* Note: Please do not put time-consuming operations in the callback function! */
/* Make a copy of the error message and process it in another thread is recommended*/
RS_WARNING << "Error code : " << code.toString() << RS_REND;
RS_WARNING << code.toString() << RS_REND;
}

int main(int argc, char* argv[])
Expand Down

0 comments on commit aff2e39

Please sign in to comment.