00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036
00037
00038
00039
00040
00041
00042
00043
00044
00045 #include "laser_filters/box_filter.h"
00046 #include <ros/ros.h>
00047
00048 laser_filters::LaserScanBoxFilter::LaserScanBoxFilter(){
00049
00050 }
00051
00052 bool laser_filters::LaserScanBoxFilter::configure(){
00053 up_and_running_ = true;
00054 double min_x = 0, min_y = 0, min_z = 0, max_x = 0, max_y = 0, max_z = 0;
00055 bool box_frame_set = getParam("box_frame", box_frame_);
00056 bool x_max_set = getParam("max_x", max_x);
00057 bool y_max_set = getParam("max_y", max_y);
00058 bool z_max_set = getParam("max_z", max_z);
00059 bool x_min_set = getParam("min_x", min_x);
00060 bool y_min_set = getParam("min_y", min_y);
00061 bool z_min_set = getParam("min_z", min_z);
00062 bool invert_set = getParam("invert", invert_filter);
00063
00064 ROS_INFO("BOX filter started");
00065
00066 max_.setX(max_x);
00067 max_.setY(max_y);
00068 max_.setZ(max_z);
00069 min_.setX(min_x);
00070 min_.setY(min_y);
00071 min_.setZ(min_z);
00072
00073 if(!box_frame_set){
00074 ROS_ERROR("box_frame is not set!");
00075 }
00076 if(!x_max_set){
00077 ROS_ERROR("max_x is not set!");
00078 }
00079 if(!y_max_set){
00080 ROS_ERROR("max_y is not set!");
00081 }
00082 if(!z_max_set){
00083 ROS_ERROR("max_z is not set!");
00084 }
00085 if(!x_min_set){
00086 ROS_ERROR("min_x is not set!");
00087 }
00088 if(!y_min_set){
00089 ROS_ERROR("min_y is not set!");
00090 }
00091 if(!z_min_set){
00092 ROS_ERROR("min_z is not set!");
00093 }
00094 if(!invert_set){
00095 ROS_INFO("invert filter not set, assuming false");
00096 invert_filter=false;
00097 }
00098
00099
00100 return box_frame_set && x_max_set && y_max_set && z_max_set &&
00101 x_min_set && y_min_set && z_min_set;
00102
00103 }
00104
00105 bool laser_filters::LaserScanBoxFilter::update(
00106 const sensor_msgs::LaserScan& input_scan,
00107 sensor_msgs::LaserScan &output_scan)
00108 {
00109 output_scan = input_scan;
00110 sensor_msgs::PointCloud2 laser_cloud;
00111
00112 std::string error_msg;
00113
00114 bool success = tf_.waitForTransform(
00115 box_frame_,
00116 input_scan.header.frame_id,
00117 input_scan.header.stamp + ros::Duration().fromSec(input_scan.ranges.size()*input_scan.time_increment),
00118 ros::Duration(1.0),
00119 ros::Duration(0.01),
00120 &error_msg
00121 );
00122 if(!success){
00123 ROS_WARN("Could not get transform, irgnoring laser scan! %s", error_msg.c_str());
00124 return false;
00125 }
00126
00127 try{
00128 projector_.transformLaserScanToPointCloud(box_frame_, input_scan, laser_cloud, tf_);
00129 }
00130 catch(tf::TransformException& ex){
00131 if(up_and_running_){
00132 ROS_WARN_THROTTLE(1, "Dropping Scan: Tansform unavailable %s", ex.what());
00133 return true;
00134 }
00135 else
00136 {
00137 ROS_INFO_THROTTLE(.3, "Ignoring Scan: Waiting for TF");
00138 }
00139 return false;
00140 }
00141 const int i_idx_c = sensor_msgs::getPointCloud2FieldIndex(laser_cloud, "index");
00142 const int x_idx_c = sensor_msgs::getPointCloud2FieldIndex(laser_cloud, "x");
00143 const int y_idx_c = sensor_msgs::getPointCloud2FieldIndex(laser_cloud, "y");
00144 const int z_idx_c = sensor_msgs::getPointCloud2FieldIndex(laser_cloud, "z");
00145
00146 if(i_idx_c == -1 || x_idx_c == -1 || y_idx_c == -1 || z_idx_c == -1){
00147 ROS_INFO_THROTTLE(.3, "x, y, z and index fields are required, skipping scan");
00148
00149 }
00150
00151
00152 const int i_idx_offset = laser_cloud.fields[i_idx_c].offset;
00153 const int x_idx_offset = laser_cloud.fields[x_idx_c].offset;
00154 const int y_idx_offset = laser_cloud.fields[y_idx_c].offset;
00155 const int z_idx_offset = laser_cloud.fields[z_idx_c].offset;
00156
00157 const int pstep = laser_cloud.point_step;
00158 const long int pcount = laser_cloud.width * laser_cloud.height;
00159 const long int limit = pstep * pcount;
00160
00161 int i_idx, x_idx, y_idx, z_idx;
00162 for(
00163 i_idx = i_idx_offset,
00164 x_idx = x_idx_offset,
00165 y_idx = y_idx_offset,
00166 z_idx = z_idx_offset;
00167
00168 x_idx < limit;
00169
00170 i_idx += pstep,
00171 x_idx += pstep,
00172 y_idx += pstep,
00173 z_idx += pstep)
00174 {
00175
00176
00177
00178 float x = *((float*)(&laser_cloud.data[x_idx]));
00179 float y = *((float*)(&laser_cloud.data[y_idx]));
00180 float z = *((float*)(&laser_cloud.data[z_idx]));
00181 int index = *((int*)(&laser_cloud.data[i_idx]));
00182
00183 tf::Point point(x, y, z);
00184
00185 if(!invert_filter){
00186 if(inBox(point)){
00187 output_scan.ranges[index] = std::numeric_limits<float>::quiet_NaN();
00188 }
00189 }
00190 else{
00191 if(!inBox(point)){
00192 output_scan.ranges[index] = std::numeric_limits<float>::quiet_NaN();
00193 }
00194 }
00195
00196 }
00197 up_and_running_ = true;
00198 return true;
00199 }
00200
00201 bool laser_filters::LaserScanBoxFilter::inBox(tf::Point &point){
00202 return
00203 point.x() < max_.x() && point.x() > min_.x() &&
00204 point.y() < max_.y() && point.y() > min_.y() &&
00205 point.z() < max_.z() && point.z() > min_.z();
00206 }
00207