55 dyn_server_.reset(
new dynamic_reconfigure::Server<BoxFilterConfig>(own_mutex_, private_nh));
56 dynamic_reconfigure::Server<BoxFilterConfig>::CallbackType
f;
57 f = [
this](
auto& config,
auto level){ reconfigureCB(config, level); };
58 dyn_server_->setCallback(
f);
60 up_and_running_ =
true;
62 getParam(
"box_frame", config_.box_frame);
63 getParam(
"max_x", config_.max_x);
64 getParam(
"max_y", config_.max_y);
65 getParam(
"max_z", config_.max_z);
66 getParam(
"min_x", config_.min_x);
67 getParam(
"min_y", config_.min_y);
68 getParam(
"min_z", config_.min_z);
69 getParam(
"invert", config_.invert);
70 dyn_server_->updateConfig(config_);
73 ROS_INFO(
"Box frame is: %s", config_.box_frame.c_str());
74 ROS_INFO(
"Box: x_min %f, x_max, %f, y_min, %f, y_max, %f, min_z %f, max_z %f", config_.min_x,
75 config_.max_x, config_.min_y, config_.max_y, config_.min_z, config_.max_z);
76 ROS_INFO(
"Box filter invert: %d", config_.invert);
81 const sensor_msgs::LaserScan& input_scan,
82 sensor_msgs::LaserScan &output_scan)
84 output_scan = input_scan;
85 sensor_msgs::PointCloud2 laser_cloud;
87 std::string error_msg;
89 bool success = tf_.waitForTransform(
91 input_scan.header.frame_id,
92 input_scan.header.stamp +
ros::Duration().
fromSec(input_scan.ranges.size()*input_scan.time_increment),
98 ROS_WARN(
"Could not get transform, irgnoring laser scan! %s", error_msg.c_str());
103 projector_.transformLaserScanToPointCloud(config_.box_frame, input_scan, laser_cloud, tf_);
121 if(i_idx_c == -1 || x_idx_c == -1 || y_idx_c == -1 || z_idx_c == -1){
127 const int i_idx_offset = laser_cloud.fields[i_idx_c].offset;
128 const int x_idx_offset = laser_cloud.fields[x_idx_c].offset;
129 const int y_idx_offset = laser_cloud.fields[y_idx_c].offset;
130 const int z_idx_offset = laser_cloud.fields[z_idx_c].offset;
132 const int pstep = laser_cloud.point_step;
133 const long int pcount = laser_cloud.width * laser_cloud.height;
134 const long int limit = pstep * pcount;
136 int i_idx, x_idx, y_idx, z_idx;
138 i_idx = i_idx_offset,
139 x_idx = x_idx_offset,
140 y_idx = y_idx_offset,
141 z_idx = z_idx_offset;
153 float x = *((
float*)(&laser_cloud.data[x_idx]));
154 float y = *((
float*)(&laser_cloud.data[y_idx]));
155 float z = *((
float*)(&laser_cloud.data[z_idx]));
156 int index = *((
int*)(&laser_cloud.data[i_idx]));
162 output_scan.ranges[index] = std::numeric_limits<float>::quiet_NaN();
167 output_scan.ranges[index] = std::numeric_limits<float>::quiet_NaN();
172 up_and_running_ =
true;
178 point.x() < config_.max_x && point.x() > config_.min_x &&
179 point.y() < config_.max_y && point.y() > config_.min_y &&
180 point.z() < config_.max_z && point.z() > config_.min_z;