Skip to content

Commit

Permalink
refactor(velodyne): improve invalid point remove
Browse files Browse the repository at this point in the history 
Signed-off-by: Mehmet Emin BAŞOĞLU <[email protected]>
  • Loading branch information
@mebasoglu
mebasoglu committed Feb 3, 2024
1 parent 90e6ca1 commit 5cdce1a
Show file tree
Hide file tree
Showing 8 changed files with 43 additions and 97 deletions.
3 changes: 1 addition & 2 deletions nebula_common/include/nebula_common/velodyne/velodyne_common.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -14,7 +14,6 @@ namespace drivers

/// @brief Invalid region on the cloud to be removed
struct InvalidRegion {
int ring;
uint16_t start;
uint16_t end;
};
Expand All @@ -28,7 +27,7 @@ struct VelodyneSensorConfiguration : SensorConfigurationBase
uint16_t cloud_min_angle;
uint16_t cloud_max_angle;
bool invalid_point_remove;
std::vector<InvalidRegion> invalid_regions;
std::map<int, std::vector<InvalidRegion>> invalid_regions;
};
/// @brief Convert VelodyneSensorConfiguration to string (Overloading the << operator)
/// @param os
Expand Down
11 changes: 4 additions & 7 deletions ...oders/include/nebula_decoders/nebula_decoders_velodyne/decoders/velodyne_scan_decoder.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -195,13 +195,10 @@ class VelodyneScanDecoder
bool check_invalid_point(const int & channel, const uint16_t & azimuth)
{
if (!sensor_configuration_->invalid_point_remove) return false;

for (const InvalidRegion & region : sensor_configuration_->invalid_regions) {
if (region.ring == channel && azimuth >= region.start && azimuth <= region.end) {
return true;
}
}
return false;
const auto & regions = sensor_configuration_->invalid_regions[channel];
return std::any_of(regions.begin(), regions.end(), [azimuth](const auto & region) {
return azimuth >= region.start && azimuth <= region.end;
});
}
};

Expand Down
8 changes: 3 additions & 5 deletions nebula_decoders/src/nebula_decoders_velodyne/decoders/vlp16_decoder.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -236,12 +236,13 @@ void Vlp16Decoder::unpack(const velodyne_msgs::msg::VelodynePacket & velodyne_pa
// Condition added to avoid calculating points which are not in the interesting
// defined area (min_angle < area < max_angle).
if (
(azimuth_corrected >= sensor_configuration_->cloud_min_angle * 100 &&
((azimuth_corrected >= sensor_configuration_->cloud_min_angle * 100 &&
azimuth_corrected <= sensor_configuration_->cloud_max_angle * 100 &&
sensor_configuration_->cloud_min_angle < sensor_configuration_->cloud_max_angle) ||
(sensor_configuration_->cloud_min_angle > sensor_configuration_->cloud_max_angle &&
(azimuth_corrected <= sensor_configuration_->cloud_max_angle * 100 ||
azimuth_corrected >= sensor_configuration_->cloud_min_angle * 100))) {
azimuth_corrected >= sensor_configuration_->cloud_min_angle * 100))) &&
!check_invalid_point(corrections.laser_ring, azimuth_corrected)) {
// Convert polar coordinates to Euclidean XYZ.
const float cos_vert_angle = corrections.cos_vert_correction;
const float sin_vert_angle = corrections.sin_vert_correction;
Expand Down Expand Up @@ -313,9 +314,6 @@ void Vlp16Decoder::unpack(const velodyne_msgs::msg::VelodynePacket & velodyne_pa
current_point.return_type = return_type;
current_point.channel = corrections.laser_ring;
current_point.azimuth = rotation_radians_[azimuth_corrected];

if (check_invalid_point(corrections.laser_ring, azimuth_corrected)) continue;

current_point.elevation = sin_vert_angle;
auto point_ts = block_timestamp - scan_timestamp_ + point_time_offset;
if (point_ts < 0) point_ts = 0;
Expand Down
8 changes: 3 additions & 5 deletions nebula_decoders/src/nebula_decoders_velodyne/decoders/vlp32_decoder.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -193,12 +193,13 @@ void Vlp32Decoder::unpack(const velodyne_msgs::msg::VelodynePacket & velodyne_pa
/*condition added to avoid calculating points which are not
in the interesting defined area (min_angle < area < max_angle)*/
if (
(block.rotation >= sensor_configuration_->cloud_min_angle * 100 &&
((block.rotation >= sensor_configuration_->cloud_min_angle * 100 &&
block.rotation <= sensor_configuration_->cloud_max_angle * 100 &&
sensor_configuration_->cloud_min_angle < sensor_configuration_->cloud_max_angle) ||
(sensor_configuration_->cloud_min_angle > sensor_configuration_->cloud_max_angle &&
(raw->blocks[i].rotation <= sensor_configuration_->cloud_max_angle * 100 ||
raw->blocks[i].rotation >= sensor_configuration_->cloud_min_angle * 100))) {
raw->blocks[i].rotation >= sensor_configuration_->cloud_min_angle * 100))) &&
!check_invalid_point(corrections.laser_ring, block.rotation)) {
const float cos_vert_angle = corrections.cos_vert_correction;
const float sin_vert_angle = corrections.sin_vert_correction;
const float cos_rot_correction = corrections.cos_rot_correction;
Expand Down Expand Up @@ -340,9 +341,6 @@ void Vlp32Decoder::unpack(const velodyne_msgs::msg::VelodynePacket & velodyne_pa
current_point.return_type = static_cast<uint8_t>(return_type);
current_point.channel = corrections.laser_ring;
current_point.azimuth = rotation_radians_[block.rotation];

if (check_invalid_point(corrections.laser_ring, block.rotation)) continue;

current_point.elevation = sin_vert_angle;
auto point_ts = block_timestamp - scan_timestamp_ + point_time_offset;
if (point_ts < 0) point_ts = 0;
Expand Down
8 changes: 3 additions & 5 deletions nebula_decoders/src/nebula_decoders_velodyne/decoders/vls128_decoder.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -257,12 +257,13 @@ void Vls128Decoder::unpack(const velodyne_msgs::msg::VelodynePacket & velodyne_p
// Condition added to avoid calculating points which are not in the interesting defined
// area (cloud_min_angle < area < cloud_max_angle).
if (
(azimuth_corrected >= sensor_configuration_->cloud_min_angle * 100 &&
((azimuth_corrected >= sensor_configuration_->cloud_min_angle * 100 &&
azimuth_corrected <= sensor_configuration_->cloud_max_angle * 100 &&
sensor_configuration_->cloud_min_angle < sensor_configuration_->cloud_max_angle) ||
(sensor_configuration_->cloud_min_angle > sensor_configuration_->cloud_max_angle &&
(azimuth_corrected <= sensor_configuration_->cloud_max_angle * 100 ||
azimuth_corrected >= sensor_configuration_->cloud_min_angle * 100))) {
azimuth_corrected >= sensor_configuration_->cloud_min_angle * 100))) &&
!check_invalid_point(corrections.laser_ring, azimuth_corrected)) {
// convert polar coordinates to Euclidean XYZ.
const float cos_vert_angle = corrections.cos_vert_correction;
const float sin_vert_angle = corrections.sin_vert_correction;
Expand Down Expand Up @@ -332,9 +333,6 @@ void Vls128Decoder::unpack(const velodyne_msgs::msg::VelodynePacket & velodyne_p
current_point.return_type = return_type;
current_point.channel = corrections.laser_ring;
current_point.azimuth = rotation_radians_[azimuth_corrected];

if (check_invalid_point(corrections.laser_ring, azimuth_corrected)) continue;

current_point.elevation = sin_vert_angle;
current_point.distance = distance;
auto point_ts = block_timestamp - scan_timestamp_ + point_time_offset;
Expand Down
3 changes: 3 additions & 0 deletions nebula_ros/include/nebula_ros/velodyne/velodyne_decoder_ros_wrapper.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -15,6 +15,9 @@
#include <velodyne_msgs/msg/velodyne_packet.hpp>
#include <velodyne_msgs/msg/velodyne_scan.hpp>

#include "boost/property_tree/json_parser.hpp"
#include "boost/property_tree/ptree.hpp"

namespace nebula
{
namespace ros
Expand Down
14 changes: 3 additions & 11 deletions nebula_ros/launch/velodyne_launch_all_hw.xml
View file
Original file line number Diff line number Diff line change
Expand Up @@ -20,14 +20,8 @@
<arg name="setup_sensor" default="True" description="Enable sensor setup on hw-driver."/>

<arg name="invalid_point_remove" default="true"/>

<!-- <arg name="invalid_rings" default="[0]"/>-->
<!-- <arg name="invalid_angles_start" default="[0]"/>-->
<!-- <arg name="invalid_angles_end" default="[0]"/>-->

<arg name="invalid_rings" default="[12,0,1,2,3,4,5,6,0,1,2,3,4]"/>
<arg name="invalid_angles_start" default="[31110,3500,3400,3200,3200,3000,2950,3100,26000,26000,26000,26000,26000]"/>
<arg name="invalid_angles_end" default="[31495,6900,6500,4600,4600,4300,4050,3800,32200,32200,31000,30000,28000]"/>
<arg name="invalid_regions"
default="'[[12, 31110, 31495], [0, 3500, 6900], [1, 3400, 6500], [2, 3200, 4600], [3, 3200, 4600], [4, 3000, 4300], [5, 2950, 4050], [6, 3100, 3800], [0, 26000, 32200], [1, 26000, 32200], [2, 26000, 31000], [3, 26000, 30000], [4, 26000, 28000]]'"/>

<node pkg="nebula_ros" exec="velodyne_driver_ros_wrapper_node"
name="velodyne_cloud" output="screen">
Expand All @@ -37,9 +31,7 @@
<param name="scan_phase" value="$(var scan_phase)"/>
<param name="calibration_file" value="$(var calibration_file)"/>
<param name="invalid_point_remove" value="$(var invalid_point_remove)"/>
<param name="invalid_rings" value="$(var invalid_rings)"/>
<param name="invalid_angles_start" value="$(var invalid_angles_start)"/>
<param name="invalid_angles_end" value="$(var invalid_angles_end)"/>
<param name="invalid_regions" value="$(var invalid_regions)"/>
</node>

<node pkg="nebula_ros" exec="velodyne_hw_ros_wrapper_node"
Expand Down
85 changes: 23 additions & 62 deletions nebula_ros/src/velodyne/velodyne_decoder_ros_wrapper.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -255,82 +255,43 @@ Status VelodyneDriverRosWrapper::GetParameters(
this->declare_parameter<bool>("invalid_point_remove", false, descriptor);
sensor_configuration.invalid_point_remove =
this->get_parameter("invalid_point_remove").as_bool();

if (sensor_configuration.invalid_point_remove) {
RCLCPP_INFO_STREAM(this->get_logger(), "Invalid point remove is active.");
} else {
RCLCPP_INFO_STREAM(this->get_logger(), "Invalid point remove is not active.");
}
}

std::vector<int64_t> invalid_rings;
std::vector<int64_t> invalid_angles_start;
std::vector<int64_t> invalid_angles_end;

{
rcl_interfaces::msg::ParameterDescriptor descriptor;
descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER_ARRAY;
descriptor.read_only = false;
descriptor.dynamic_typing = false;
descriptor.additional_constraints = "";
this->declare_parameter<std::vector<int64_t>>("invalid_rings", descriptor);
invalid_rings = this->get_parameter("invalid_rings").as_integer_array();

RCLCPP_INFO_STREAM(this->get_logger(), "Invalid rings: ");
for (const auto & invalid_ring : invalid_rings) {
RCLCPP_INFO_STREAM(this->get_logger(), invalid_ring << " ");
}
}

{
rcl_interfaces::msg::ParameterDescriptor descriptor;
descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER_ARRAY;
descriptor.read_only = false;
descriptor.dynamic_typing = false;
descriptor.additional_constraints = "";
this->declare_parameter<std::vector<int64_t>>("invalid_angles_start", descriptor);
invalid_angles_start = this->get_parameter("invalid_angles_start").as_integer_array();

RCLCPP_INFO_STREAM(this->get_logger(), "Invalid angles start: ");
for (const auto & invalid_angle_start : invalid_angles_start) {
RCLCPP_INFO_STREAM(this->get_logger(), invalid_angle_start << " ");
}
}

{
rcl_interfaces::msg::ParameterDescriptor descriptor;
descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER_ARRAY;
descriptor.read_only = false;
descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_STRING;
descriptor.read_only = true;
descriptor.dynamic_typing = false;
descriptor.additional_constraints = "";
this->declare_parameter<std::vector<int64_t>>("invalid_angles_end", descriptor);
invalid_angles_end = this->get_parameter("invalid_angles_end").as_integer_array();

RCLCPP_INFO_STREAM(this->get_logger(), "Invalid angles end: ");
for (const auto & invalid_angle_end : invalid_angles_end) {
RCLCPP_INFO_STREAM(this->get_logger(), invalid_angle_end << " ");
}
}

if (sensor_configuration.invalid_point_remove) {
// Check length of invalid_rings, invalid_angles_start and invalid_angles_end
if (
invalid_rings.size() != invalid_angles_start.size() ||
invalid_rings.size() != invalid_angles_end.size()) {
RCLCPP_ERROR_STREAM(
this->get_logger(),
"Invalid rings, invalid angles start and invalid angles end must have the same length.");
return Status::SENSOR_CONFIG_ERROR;
this->declare_parameter<std::string>("invalid_regions", "", descriptor);
const std::string regions_string = this->get_parameter("invalid_regions").as_string();
std::istringstream regions_stream(regions_string);

boost::property_tree::ptree pt;
boost::property_tree::read_json(regions_stream, pt);

for (const auto & region : pt) {
std::vector<int> elements; // Will store extracted ring number, start angle and end angle.
for (const auto & element : region.second) {
elements.push_back(element.second.get<int>(""));
}
sensor_configuration.invalid_regions[elements[0]].push_back(
{static_cast<uint16_t>(elements[1]), static_cast<uint16_t>(elements[2])});
}

// Add invalid regions to sensor configuration
for (size_t i = 0; i < invalid_rings.size(); i++) {
drivers::InvalidRegion invalid_region;
invalid_region.ring = invalid_rings[i];
invalid_region.start = invalid_angles_start[i];
invalid_region.end = invalid_angles_end[i];
sensor_configuration.invalid_regions.push_back(invalid_region);
std::stringstream regions_log;
for (const auto & regions : sensor_configuration.invalid_regions) {
regions_log << "\nRing: " << regions.first << '\n';
for (const auto & region : regions.second) {
regions_log << "(" << region.start << "," << region.end << ")\n";
}
}
RCLCPP_INFO_STREAM(get_logger(), regions_log.str());
}

if (sensor_configuration.sensor_model == nebula::drivers::SensorModel::UNKNOWN) {
Expand Down

0 comments on commit 5cdce1a

Please sign in to comment.