54 double min_x = 0, min_y = 0, min_z = 0, max_x = 0, max_y = 0, max_z = 0;
56 bool x_max_set =
getParam(
"max_x", max_x);
57 bool y_max_set =
getParam(
"max_y", max_y);
58 bool z_max_set =
getParam(
"max_z", max_z);
59 bool x_min_set =
getParam(
"min_x", min_x);
60 bool y_min_set =
getParam(
"min_y", min_y);
61 bool z_min_set =
getParam(
"min_z", min_z);
95 ROS_INFO(
"invert filter not set, assuming false");
100 return box_frame_set && x_max_set && y_max_set && z_max_set &&
101 x_min_set && y_min_set && z_min_set;
106 const sensor_msgs::LaserScan& input_scan,
107 sensor_msgs::LaserScan &output_scan)
109 output_scan = input_scan;
110 sensor_msgs::PointCloud2 laser_cloud;
112 std::string error_msg;
116 input_scan.header.frame_id,
117 input_scan.header.stamp +
ros::Duration().
fromSec(input_scan.ranges.size()*input_scan.time_increment),
123 ROS_WARN(
"Could not get transform, irgnoring laser scan! %s", error_msg.c_str());
146 if(i_idx_c == -1 || x_idx_c == -1 || y_idx_c == -1 || z_idx_c == -1){
152 const int i_idx_offset = laser_cloud.fields[i_idx_c].offset;
153 const int x_idx_offset = laser_cloud.fields[x_idx_c].offset;
154 const int y_idx_offset = laser_cloud.fields[y_idx_c].offset;
155 const int z_idx_offset = laser_cloud.fields[z_idx_c].offset;
157 const int pstep = laser_cloud.point_step;
158 const long int pcount = laser_cloud.width * laser_cloud.height;
159 const long int limit = pstep * pcount;
161 int i_idx, x_idx, y_idx, z_idx;
163 i_idx = i_idx_offset,
164 x_idx = x_idx_offset,
165 y_idx = y_idx_offset,
166 z_idx = z_idx_offset;
178 float x = *((
float*)(&laser_cloud.data[x_idx]));
179 float y = *((
float*)(&laser_cloud.data[y_idx]));
180 float z = *((
float*)(&laser_cloud.data[z_idx]));
181 int index = *((
int*)(&laser_cloud.data[i_idx]));
187 output_scan.ranges[index] = std::numeric_limits<float>::quiet_NaN();
192 output_scan.ranges[index] = std::numeric_limits<float>::quiet_NaN();
static int getPointCloud2FieldIndex(const sensor_msgs::PointCloud2 &cloud, const std::string &field_name)
TFSIMD_FORCE_INLINE void setX(tfScalar x)
TFSIMD_FORCE_INLINE void setY(tfScalar y)
TFSIMD_FORCE_INLINE void setZ(tfScalar z)
laser_geometry::LaserProjection projector_
TFSIMD_FORCE_INLINE const tfScalar & y() const
tf::TransformListener tf_
bool getParam(const std::string &name, std::string &value)
TFSIMD_FORCE_INLINE const tfScalar & x() const
#define ROS_INFO_THROTTLE(period,...)
TFSIMD_FORCE_INLINE const tfScalar & z() const
#define ROS_WARN_THROTTLE(period,...)
Duration & fromSec(double t)
bool inBox(tf::Point &point)
TFSIMD_FORCE_INLINE const tfScalar & y() const
TFSIMD_FORCE_INLINE const tfScalar & x() const
TFSIMD_FORCE_INLINE const tfScalar & z() const
bool update(const sensor_msgs::LaserScan &input_scan, sensor_msgs::LaserScan &filtered_scan)
void transformLaserScanToPointCloud(const std::string &target_frame, const sensor_msgs::LaserScan &scan_in, sensor_msgs::PointCloud &cloud_out, tf::Transformer &tf, double range_cutoff, int channel_options=channel_option::Default)