00001 //================================================================================================= 00002 // Copyright (c) 2011, Stefan Kohlbrecher, TU Darmstadt 00003 // All rights reserved. 00004 00005 // Redistribution and use in source and binary forms, with or without 00006 // modification, are permitted provided that the following conditions are met: 00007 // * Redistributions of source code must retain the above copyright 00008 // notice, this list of conditions and the following disclaimer. 00009 // * Redistributions in binary form must reproduce the above copyright 00010 // notice, this list of conditions and the following disclaimer in the 00011 // documentation and/or other materials provided with the distribution. 00012 // * Neither the name of the Simulation, Systems Optimization and Robotics 00013 // group, TU Darmstadt nor the names of its contributors may be used to 00014 // endorse or promote products derived from this software without 00015 // specific prior written permission. 00016 00017 // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 00018 // ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 00019 // WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 00020 // DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER BE LIABLE FOR ANY 00021 // DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 00022 // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 00023 // LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 00024 // ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 00025 // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 00026 // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 00027 //================================================================================================= 00028 00029 #include <ros/ros.h> 00030 00031 #include <hector_thermal_self_filter/hector_thermal_self_filter.h> 00032 #include <hector_worldmodel_msgs/VerifyPercept.h> 00033 00034 namespace hector_thermal_self_filter{ 00035 00036 class ThermalSelfFilter 00037 { 00038 public: 00039 ThermalSelfFilter() 00040 { 00041 ros::NodeHandle pnh("~"); 00042 self_filter_ = new HectorThermalSelfFilter(pnh, &tfL_); 00043 00044 ooi_verification_service_ = pnh.advertiseService("verify_percept", &ThermalSelfFilter::verifyPerceptCallBack, this); 00045 } 00046 00047 ~ThermalSelfFilter() 00048 { 00049 delete self_filter_; 00050 } 00051 00052 bool verifyPerceptCallBack(hector_worldmodel_msgs::VerifyPercept::Request &req, 00053 hector_worldmodel_msgs::VerifyPercept::Response &res ) 00054 { 00055 bool belongsToRobot = self_filter_->pointBelongsToRobot(req.percept.pose.pose.position, req.percept.header); 00056 if (req.percept.info.class_id == "qrcode"){ 00057 res.response = hector_worldmodel_msgs::VerifyPerceptResponse::UNKNOWN; 00058 return true; 00059 } 00060 if (req.percept.info.class_id == "barrel"){ 00061 res.response = hector_worldmodel_msgs::VerifyPerceptResponse::UNKNOWN; 00062 return true; 00063 } 00064 if (belongsToRobot){ 00065 res.response = hector_worldmodel_msgs::VerifyPerceptResponse::DISCARD; 00066 }else{ 00067 res.response = hector_worldmodel_msgs::VerifyPerceptResponse::UNKNOWN; 00068 } 00069 00070 return true; 00071 } 00072 00073 protected: 00074 HectorThermalSelfFilter* self_filter_; 00075 tf::TransformListener tfL_; 00076 00077 ros::ServiceServer ooi_verification_service_; 00078 00079 }; 00080 00081 } 00082 00083 int main(int argc, char** argv) 00084 { 00085 ros::init(argc, argv, "thermal_self_filter"); 00086 00087 hector_thermal_self_filter::ThermalSelfFilter sf; 00088 00089 ros::spin(); 00090 00091 return 0; 00092 }