Skip to content

Add binning and box masking. #14

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

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

Add binning and box masking. #14

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
e522ca3
Add binning and box masking.
jgoppert Nov 16, 2023
4bb7c93
Add min/max range filter.
jgoppert Nov 16, 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
10 changes: 9 additions & 1 deletion include/ros2_api.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -30,11 +30,19 @@ struct LaserScanSetting
std::string frame_id;
bool laser_scan_dir;
bool enable_angle_crop_func;
bool enable_box_crop_func;
double angle_crop_min;
double angle_crop_max;
double x_crop_min;
double x_crop_max;
double y_crop_min;
double y_crop_max;
int bins;
double range_min;
double range_max;
};

#endif //__ROS_API_H__

/********************* (C) COPYRIGHT SHENZHEN LDROBOT CO., LTD *******END OF
* FILE ********/
* FILE ********/
71 changes: 60 additions & 11 deletions src/demo.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -31,16 +31,24 @@ int main(int argc, char **argv) {
rclcpp::init(argc, argv);
auto node = std::make_shared<rclcpp::Node>("ldlidar_published"); // create a ROS2 Node
std::string product_name;
std::string topic_name;
std::string port_name;
int serial_port_baudrate;
std::string topic_name;
std::string port_name;
int serial_port_baudrate = 0;
ldlidar::LDType type_name;
LaserScanSetting setting;
setting.frame_id = "base_laser";
setting.frame_id = "base_laser";
setting.laser_scan_dir = true;
setting.enable_angle_crop_func = false;
setting.angle_crop_min = 0.0;
setting.angle_crop_max = 0.0;
setting.bins = 0;
setting.enable_box_crop_func = false;
setting.x_crop_min = 0.0;
setting.x_crop_max = 0.0;
setting.y_crop_min = 0.0;
setting.y_crop_max = 0.0;
setting.range_min = 0.03;
setting.range_max = 25.0;

// declare ros2 param
node->declare_parameter<std::string>("product_name", product_name);
Expand All @@ -52,6 +60,14 @@ int main(int argc, char **argv) {
node->declare_parameter<bool>("enable_angle_crop_func", setting.enable_angle_crop_func);
node->declare_parameter<double>("angle_crop_min", setting.angle_crop_min);
node->declare_parameter<double>("angle_crop_max", setting.angle_crop_max);
node->declare_parameter<int>("bins", setting.bins);
node->declare_parameter<bool>("enable_box_crop_func", setting.enable_box_crop_func);
node->declare_parameter<double>("x_crop_min", setting.x_crop_min);
node->declare_parameter<double>("x_crop_max", setting.x_crop_max);
node->declare_parameter<double>("y_crop_min", setting.y_crop_min);
node->declare_parameter<double>("y_crop_max", setting.y_crop_max);
node->declare_parameter<double>("range_min", setting.range_min);
node->declare_parameter<double>("range_max", setting.range_max);

// get ros2 param
node->get_parameter("product_name", product_name);
Expand All @@ -63,6 +79,14 @@ int main(int argc, char **argv) {
node->get_parameter("enable_angle_crop_func", setting.enable_angle_crop_func);
node->get_parameter("angle_crop_min", setting.angle_crop_min);
node->get_parameter("angle_crop_max", setting.angle_crop_max);
node->get_parameter("bins", setting.bins);
node->get_parameter("enable_box_crop_func", setting.enable_box_crop_func);
node->get_parameter("x_crop_min", setting.x_crop_min);
node->get_parameter("x_crop_max", setting.x_crop_max);
node->get_parameter("y_crop_min", setting.y_crop_min);
node->get_parameter("y_crop_max", setting.y_crop_max);
node->get_parameter("range_min", setting.range_min);
node->get_parameter("range_max", setting.range_max);

ldlidar::LDLidarDriver* ldlidarnode = new ldlidar::LDLidarDriver();

Expand All @@ -76,6 +100,16 @@ int main(int argc, char **argv) {
RCLCPP_INFO(node->get_logger(), "<enable_angle_crop_func>: %s", (setting.enable_angle_crop_func?"true":"false"));
RCLCPP_INFO(node->get_logger(), "<angle_crop_min>: %f", setting.angle_crop_min);
RCLCPP_INFO(node->get_logger(), "<angle_crop_max>: %f", setting.angle_crop_max);
RCLCPP_INFO(node->get_logger(), "<bins>: %d", setting.bins);
RCLCPP_INFO(node->get_logger(), "<enable_box_crop_func>: %s", (setting.enable_box_crop_func?"true":"false"));
RCLCPP_INFO(node->get_logger(), "<x_crop_min>: %f", setting.x_crop_min);
RCLCPP_INFO(node->get_logger(), "<x_crop_max>: %f", setting.x_crop_max);
RCLCPP_INFO(node->get_logger(), "<y_crop_min>: %f", setting.y_crop_min);
RCLCPP_INFO(node->get_logger(), "<y_crop_max>: %f", setting.y_crop_max);

if (setting.bins > 0 && setting.bins < 10) {
RCLCPP_INFO(node->get_logger(), "recommend increasing bin number");
}

if (product_name == "LDLiDAR_LD06") {
type_name = ldlidar::LDType::LD_06;
Expand Down Expand Up @@ -146,12 +180,20 @@ int main(int argc, char **argv) {

void ToLaserscanMessagePublish(ldlidar::Points2D& src, double lidar_spin_freq, LaserScanSetting& setting,
rclcpp::Node::SharedPtr& node, rclcpp::Publisher<sensor_msgs::msg::LaserScan>::SharedPtr& lidarpub) {
float angle_min, angle_max, range_min, range_max, angle_increment;
float angle_min, angle_max, angle_increment;
double scan_time;
rclcpp::Time start_scan_time;
static rclcpp::Time end_scan_time;
static bool first_scan = true;

int beam_size = 0;

if (setting.bins > 0) {
beam_size = setting.bins;
} else {
beam_size = static_cast<int>(src.size());
}

start_scan_time = node->now();
scan_time = (start_scan_time.seconds() - end_scan_time.seconds());

Expand All @@ -163,9 +205,6 @@ void ToLaserscanMessagePublish(ldlidar::Points2D& src, double lidar_spin_freq,
// Adjust the parameters according to the demand
angle_min = 0;
angle_max = (2 * M_PI);
range_min = 0.02;
range_max = 25;
int beam_size = static_cast<int>(src.size());
angle_increment = (angle_max - angle_min) / (float)(beam_size -1);
// Calculate the number of scanning points
if (lidar_spin_freq > 0) {
Expand All @@ -174,8 +213,8 @@ void ToLaserscanMessagePublish(ldlidar::Points2D& src, double lidar_spin_freq,
output.header.frame_id = setting.frame_id;
output.angle_min = angle_min;
output.angle_max = angle_max;
output.range_min = range_min;
output.range_max = range_max;
output.range_min = setting.range_min;
output.range_max = setting.range_max;
output.angle_increment = angle_increment;
if (beam_size <= 1) {
output.time_increment = 0;
Expand Down Expand Up @@ -203,6 +242,16 @@ void ToLaserscanMessagePublish(ldlidar::Points2D& src, double lidar_spin_freq,
}
}

if (setting.enable_box_crop_func) { // Box crop setting, Mask data within the box
double x = range*cos(dir_angle*M_PI/180);
double y = -range*sin(dir_angle*M_PI/180);
if (x > setting.x_crop_min && x < setting.x_crop_max &&
y > setting.y_crop_min && y < setting.y_crop_max) {
range = std::numeric_limits<float>::quiet_NaN();
intensity = std::numeric_limits<float>::quiet_NaN();
}
}

float angle = ANGLE_TO_RADIAN(dir_angle); // Lidar angle unit form degree transform to radian
int index = static_cast<int>(ceil((angle - angle_min) / angle_increment));
if (index < beam_size) {
Expand Down Expand Up @@ -250,4 +299,4 @@ uint64_t GetSystemTimeStamp(void) {
}

/********************* (C) COPYRIGHT SHENZHEN LDROBOT CO., LTD *******END OF
* FILE ********/
* FILE ********/