Go to the documentation of this file.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 #include <XmlRpc.h>
00039 #include <sensor_msgs/LaserScan.h>
00040 #include <filters/filter_base.h>
00041 #include <pr2_msgs/LaserScannerSignal.h>
00042 #include <boost/thread/mutex.hpp>
00043
00044 namespace laser_tilt_controller_filter {
00045
00050 class LaserTiltControllerFilter : public filters::FilterBase<sensor_msgs::LaserScan>
00051 {
00052 public:
00053 LaserTiltControllerFilter(): signal_received_(false){}
00054
00055 bool loadFilterSignals(){
00056 XmlRpc::XmlRpcValue filter_sections;
00057
00058 if(!filters::FilterBase<sensor_msgs::LaserScan>::getParam("filter_sections", filter_sections)){
00059
00060 return true;
00061 }
00062
00063 if(!(filter_sections.getType() == XmlRpc::XmlRpcValue::TypeArray)){
00064 ROS_ERROR("The filter_sections must be a list of integers");
00065 return false;
00066 }
00067
00068
00069 for(int i = 0; i < filter_sections.size(); ++i){
00070 XmlRpc::XmlRpcValue section = filter_sections[i];
00071
00072
00073 if(!(section.getType() == XmlRpc::XmlRpcValue::TypeInt)){
00074 ROS_ERROR("The tilt_profile_times must be a list of integers");
00075 return false;
00076 }
00077
00078 double section_int = int(section);
00079
00080
00081 filter_signals_.push_back(section_int);
00082 }
00083
00084 return true;
00085 }
00086
00087 bool loadTiltProfileTiming(){
00088 XmlRpc::XmlRpcValue tilt_profile_times;
00089 if(!filters::FilterBase<sensor_msgs::LaserScan>::getParam("tilt_profile_times", tilt_profile_times)){
00090 ROS_ERROR("No tilt_profile_times were given");
00091 return false;
00092 }
00093
00094 if(!(tilt_profile_times.getType() == XmlRpc::XmlRpcValue::TypeArray)){
00095 ROS_ERROR("The tilt_profile_times must be a list of doubles");
00096 return false;
00097 }
00098
00099 if(tilt_profile_times.size() < 2){
00100 ROS_ERROR("The tilt_profile_times list must contain at least two values... a start and an end");
00101 return false;
00102 }
00103
00104
00105 for(int i = 0; i < tilt_profile_times.size(); ++i){
00106 XmlRpc::XmlRpcValue time_xml = tilt_profile_times[i];
00107
00108
00109 if(!(time_xml.getType() == XmlRpc::XmlRpcValue::TypeInt || time_xml.getType() == XmlRpc::XmlRpcValue::TypeDouble)){
00110 ROS_ERROR("The tilt_profile_times must be a list of doubles");
00111 return false;
00112 }
00113
00114 double time_dbl = time_xml.getType() == XmlRpc::XmlRpcValue::TypeInt ? (int)(time_xml) : (double)(time_xml);
00115
00116
00117 tilt_profile_times_.push_back(time_dbl);
00118 }
00119
00120 return true;
00121 }
00122
00123 bool configure(){
00124 ROS_DEBUG("Filtering initialized");
00125 bool success = loadTiltProfileTiming() && loadFilterSignals();
00126
00127 ros::NodeHandle n;
00128 signal_sub_ = n.subscribe("laser_tilt_controller/laser_scanner_signal", 1, &LaserTiltControllerFilter::signalCb, this);
00129 return success;
00130 }
00131
00132 void signalCb(const pr2_msgs::LaserScannerSignal::ConstPtr& signal){
00133 boost::mutex::scoped_lock lock(mutex_);
00134
00135 if(signal->signal == 0){
00136 timer_zero_ = signal->header.stamp;
00137 signal_received_ = true;
00138 }
00139 }
00140
00141 bool update(const sensor_msgs::LaserScan& scan_in, sensor_msgs::LaserScan& scan_out){
00142 ROS_DEBUG("Signal filter running, current timer %.4f, signal_received: %d", timer_zero_.toSec(), signal_received_);
00143
00144 if(!signal_received_){
00145 scan_out = scan_in;
00146 return true;
00147 }
00148
00149 boost::mutex::scoped_lock lock(mutex_);
00150 double time_diff = (scan_in.header.stamp - timer_zero_).toSec() + tilt_profile_times_.front();
00151
00152
00153 ROS_DEBUG("time_diff: %.4f, periold: %.4f", time_diff, tilt_profile_times_.back());
00154 double period_time = fmod(time_diff, tilt_profile_times_.back());
00155
00156 int profile_section = 0;
00157
00158 for(unsigned int i = 0; i < tilt_profile_times_.size() - 1; ++i){
00159 ROS_DEBUG("Checking period_time: %.4f against tilt_profile time: %.4f and next %.4f",
00160 period_time, tilt_profile_times_[i], tilt_profile_times_[i+1]);
00161 if(period_time >= tilt_profile_times_[i] && period_time < tilt_profile_times_[i + 1]){
00162 profile_section = i;
00163 }
00164 }
00165
00166 ROS_DEBUG("Profile section: %d, time: %.4f", profile_section, scan_in.header.stamp.toSec());
00167
00168
00169 for(unsigned int i = 0; i < filter_signals_.size(); ++i){
00170 if(filter_signals_[i] == profile_section){
00171 sensor_msgs::LaserScan empty_scan;
00172 empty_scan.header = scan_in.header;
00173 scan_out = empty_scan;
00174 ROS_DEBUG("Filtering out scan");
00175 return true;
00176 }
00177 }
00178
00179
00180
00181 scan_out = scan_in;
00182 return true;
00183 }
00184
00185 ~LaserTiltControllerFilter() {}
00186
00187 private:
00188 std::vector<int> filter_signals_;
00189 std::vector<double> tilt_profile_times_;
00190 ros::Time timer_zero_;
00191 boost::mutex mutex_;
00192 bool signal_received_;
00193 ros::Subscriber signal_sub_;
00194 };
00195
00196 };