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
00046
00047 #include "pcdfilter_pa/pcdfilter_pa_node.h"
00048
00049
00050 #include <parameter_pa/parameter_pa_ros.h>
00051
00052
00053 #include <string>
00054 #include <math.h>
00055
00056
00057 int main(int argc, char **argv) {
00058
00059 ros::init(argc, argv, "pcd_filter_pa_node");
00060 cPcdFilterPaNode pcd_filter;
00061
00062 ros::spin();
00063
00064 return 0;
00065 }
00066
00067
00068 cPcdFilterPaNode::cPcdFilterPaNode() {
00069 cParameterPaRos paramloader;
00070
00071 std::vector<std::string> temp_filter;
00072 paramloader.load("~/filters" , temp_filter);
00073
00074 paramloader.load("~/skip_count" , input_throttle_.skip_count_ );
00075 paramloader.load("~/skip_time" , input_throttle_.skip_time_ );
00076
00077 paramloader.load("~/tf_lookup_time" , filters_.tf_lookup_time_ );
00078 paramloader.load("~/buffer_pointcloud", rosparams_.buffer_pointcloud_);
00079 paramloader.load("~/debugging" , rosparams_.debugging_ );
00080 paramloader.load("~/laser_nan_replacement_value",
00081 rosparams_.laser_nan_replacement_value_);
00082
00083 bool enabled = true;
00084 paramloader.load("~/enabled" , enabled );
00085
00086
00087 bool remapping = false;
00088 remapping|= paramloader.loadTopic("~/topic_in_cloud" ,
00089 nodeparams_.topic_in_cloud_ );
00090 remapping|= paramloader.loadTopic("~/topic_in_cloud_old",
00091 nodeparams_.topic_in_cloud_old_ );
00092 remapping|= paramloader.loadTopic("~/topic_in_laser" ,
00093 nodeparams_.topic_in_laser_ );
00094
00095 paramloader.loadTopic("~/topic_out_cloud" ,
00096 nodeparams_.topic_out_cloud_ );
00097
00098
00099 pub_pcd_ = nh_.advertise<sensor_msgs::PointCloud2>(
00100 nodeparams_.topic_out_cloud_, 10, true);
00101
00102 std::string str_service = paramloader.resolveRessourcename("~/");
00103
00104
00105 ser_filter_ = nh_.advertiseService( str_service + "filter",
00106 &cPcdFilterPaNode::filterCallbackSrv, this);
00107
00108
00109 ser_add_filters_ = nh_.advertiseService( str_service + "add_filters",
00110 &cPcdFilterPaNode::addFiltersCallbackSrv, this);
00111
00112 ser_change_filters_ = nh_.advertiseService( str_service + "change_filters",
00113 &cPcdFilterPaNode::changeFiltersCallbackSrv, this);
00114
00115
00116 ser_enable_ = nh_.advertiseService( str_service + "enable",
00117 &cPcdFilterPaNode::enableCallbackSrv, this);
00118
00119 ser_disable_ = nh_.advertiseService( str_service + "disable",
00120 &cPcdFilterPaNode::disableCallbackSrv, this);
00121
00122 if (enabled) { enable();} else { disable();}
00123
00124 if (! remapping) {
00125 ROS_WARN_STREAM(ros::this_node::getName() <<
00126 ": no input topic was remapped\n" <<
00127 "this seems to be a mistake - try the following:\n" <<
00128 " rosrun pcdfilter_pa pcdfilter_pa_node _topic_in_cloud:=" <<
00129 "/new_in_topic _topic_out_cloud:=/new_out_topic");
00130 }
00131
00132 addFilters(temp_filter);
00133 }
00134
00135
00136 cPcdFilterPaNode::~cPcdFilterPaNode() {
00137 }
00138
00139
00140 void cPcdFilterPaNode::setCloudCallbackSub(
00141 const sensor_msgs::PointCloud2ConstPtr &msg) {
00142
00143 if (rosparams_.debugging_) {
00144 int count = (msg->width > 1 ? msg->width : 1) * msg->height;
00145 ROS_INFO("%s: #### received new pointcloud (%d)",
00146 ros::this_node::getName().data(), count);
00147 }
00148 if (! input_throttle_.checkNewInput(msg->header.stamp)) {
00149 if (rosparams_.debugging_) {
00150 ROS_INFO("%s: skipped (input throttle)",
00151 ros::this_node::getName().data());
00152 }
00153
00154 return;
00155 }
00156
00157 if (! updateTf(tf_listener_, msg->header.frame_id, msg->header.stamp)) {
00158 return;
00159 }
00160
00161 sensor_msgs::PointCloud2Ptr result;
00162 if (filterCloud(msg, result)) {
00163 if (rosparams_.debugging_) {
00164 ROS_INFO("%s: publishing filtered pointcloud",
00165 ros::this_node::getName().data());
00166 }
00167 pub_pcd_.publish(result);
00168 }
00169 }
00170
00171
00172 void cPcdFilterPaNode::setCloudOldCallbackSub(
00173 const sensor_msgs::PointCloudConstPtr &msg) {
00174
00175 setCloudCallbackSub(convertCloud(msg));
00176 }
00177
00178
00179 void cPcdFilterPaNode::setCloudLaserCallbackSub(
00180 const sensor_msgs::LaserScanConstPtr &msg) {
00181
00182 setCloudCallbackSub(convertCloud(msg));
00183 }
00184
00185
00186 bool cPcdFilterPaNode::addFiltersCallbackSrv(
00187 pcdfilter_pa::PcdFilterPaFilter::Request &req,
00188 pcdfilter_pa::PcdFilterPaFilter::Response &res) {
00189
00190 addFilters(req.new_filter);
00191 res.current_filter = filters_.get();
00192
00193 if (rosparams_.buffer_pointcloud_) {
00194 if (! updateTf(tf_listener_)) {
00195 return true;
00196 }
00197 sensor_msgs::PointCloud2Ptr cloud;
00198 if (filterCloud(cloud)){
00199 pub_pcd_.publish(cloud);
00200 }
00201 }
00202 return true;
00203 }
00204
00205
00206 bool cPcdFilterPaNode::filterCallbackSrv(
00207 pcdfilter_pa::PcdFilterPaCloud::Request &req,
00208 pcdfilter_pa::PcdFilterPaCloud::Response &res) {
00209
00210 res.ok = false;
00211
00212 sensor_msgs::PointCloud2ConstPtr msg(&req.in_cloud);
00213 sensor_msgs::PointCloud2Ptr result;
00214
00215 if (! updateTf(tf_listener_, msg->header.frame_id,
00216 msg->header.stamp)) {
00217 return true;
00218 }
00219
00220 if (filterCloud(msg, result)) {
00221 ROS_INFO("%s: service call for filtering",
00222 ros::this_node::getName().data());
00223
00224 sensor_msgs::PointCloud2_<std::allocator<void> >::_data_type temp;
00225 temp.swap(result->data);
00226 res.out_cloud = *result;
00227 res.out_cloud.data.swap(temp);
00228
00229 res.ok = true;
00230 }
00231 return true;
00232
00233 }
00234
00235
00236 void cPcdFilterPaNode::addFilters(
00237 const std::vector<std::string> &new_filters) {
00238
00239 for (int i = 0; i < new_filters.size(); i++) {
00240 if (filters_.add(new_filters[i])) {
00241 ROS_INFO("%s: added new filter definition \"%s\"",
00242 ros::this_node::getName().data(), filters_.getLast().data());
00243 } else {
00244 ROS_WARN("%s: error in filter definition \"%s\"",
00245 ros::this_node::getName().data(), filters_.getLast().data());
00246 }
00247 }
00248 }
00249
00250
00251 bool cPcdFilterPaNode::changeFiltersCallbackSrv(
00252 pcdfilter_pa::PcdFilterPaFilter::Request &req,
00253 pcdfilter_pa::PcdFilterPaFilter::Response &res) {
00254
00255 int count = filters_.size();
00256 filters_.clear();
00257
00258 ROS_INFO("%s: removed all filters (%d)",
00259 ros::this_node::getName().data(), count);
00260 return addFiltersCallbackSrv(req, res);
00261 }
00262
00263
00264 bool cPcdFilterPaNode::enableCallbackSrv(std_srvs::Empty::Request &req,
00265 std_srvs::Empty::Response &res) {
00266
00267 enable();
00268 return true;
00269 }
00270
00271
00272 bool cPcdFilterPaNode::disableCallbackSrv(std_srvs::Empty::Request &req,
00273 std_srvs::Empty::Response &res) {
00274
00275 disable();
00276 return true;
00277 }
00278
00279
00280 void cPcdFilterPaNode::enable() {
00281
00282 ROS_INFO("%s: node enabled :-)", ros::this_node::getName().data());
00283
00284
00285 if (nodeparams_.topic_in_cloud_ != "") {
00286 sub_pcd_ = nh_.subscribe<sensor_msgs::PointCloud2>(
00287 nodeparams_.topic_in_cloud_, 10,
00288 &cPcdFilterPaNode::setCloudCallbackSub, this);
00289 }
00290
00291
00292 if (nodeparams_.topic_in_cloud_old_ != "") {
00293 sub_pcd_old_ = nh_.subscribe<sensor_msgs::PointCloud>(
00294 nodeparams_.topic_in_cloud_old_, 10,
00295 &cPcdFilterPaNode::setCloudOldCallbackSub, this);
00296 }
00297
00298
00299 if (nodeparams_.topic_in_laser_ != "") {
00300 sub_laser_ = nh_.subscribe<sensor_msgs::LaserScan>(
00301 nodeparams_.topic_in_laser_, 10,
00302 &cPcdFilterPaNode::setCloudLaserCallbackSub, this);
00303 }
00304 }
00305
00306
00307 void cPcdFilterPaNode::disable() {
00308
00309 ROS_INFO("%s: node disabled :-(", ros::this_node::getName().data());
00310
00311 sub_pcd_.shutdown() ;
00312 sub_pcd_old_.shutdown();
00313 sub_laser_.shutdown() ;
00314 }