|
From: <tf...@us...> - 2009-02-24 23:34:45
|
Revision: 11693
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=11693&view=rev
Author: tfoote
Date: 2009-02-24 23:34:41 +0000 (Tue, 24 Feb 2009)
Log Message:
-----------
standardizing on passing of TiXmlElement * and getting laser median filter working as a full filter
Modified Paths:
--------------
pkg/trunk/prcore/filters/include/filters/filter_chain.h
pkg/trunk/prcore/filters/test/test_chain.cpp
pkg/trunk/util/laser_scan/CMakeLists.txt
pkg/trunk/util/laser_scan/include/laser_scan/median_filter.h
pkg/trunk/util/laser_scan/src/median_filter_node.cpp
Removed Paths:
-------------
pkg/trunk/util/laser_scan/src/median_filter.cpp
Modified: pkg/trunk/prcore/filters/include/filters/filter_chain.h
===================================================================
--- pkg/trunk/prcore/filters/include/filters/filter_chain.h 2009-02-24 23:31:31 UTC (rev 11692)
+++ pkg/trunk/prcore/filters/include/filters/filter_chain.h 2009-02-24 23:34:41 UTC (rev 11693)
@@ -49,10 +49,10 @@
/** \brief Configure the filter chain
* This will call configure on all filters which have been added
* as well as allocate the buffers*/
- bool configure(unsigned int size, TiXmlDocument& doc)
+ bool configure(unsigned int size, TiXmlElement* config_arg)
{
/*************************** Parse the XML ***********************************/
- TiXmlElement *config = doc.RootElement();
+ TiXmlElement *config = config_arg;
//Verify incoming xml for proper naming and structure
if (!config)
@@ -109,7 +109,7 @@
bool result = true;
- config = doc.RootElement();
+ config = config_arg;
//Step into the filter list if necessary
if (config->ValueStr() == "filters")
Modified: pkg/trunk/prcore/filters/test/test_chain.cpp
===================================================================
--- pkg/trunk/prcore/filters/test/test_chain.cpp 2009-02-24 23:31:31 UTC (rev 11692)
+++ pkg/trunk/prcore/filters/test/test_chain.cpp 2009-02-24 23:34:41 UTC (rev 11693)
@@ -78,7 +78,8 @@
// EXPECT_TRUE(chain.add(median_filter_5));
TiXmlDocument chain_def = TiXmlDocument();
chain_def.Parse(median_filter_5.c_str());
- EXPECT_TRUE(chain.configure(5, chain_def));
+ TiXmlElement * config = chain_def.RootElement();
+ EXPECT_TRUE(chain.configure(5, config));
float input1[] = {1,2,3,4,5};
float input1a[] = {9,9,9,9,9};//seed w/incorrect values
@@ -104,7 +105,8 @@
//EXPECT_TRUE(chain.add(median_filter_5));
TiXmlDocument chain_def = TiXmlDocument();
chain_def.Parse(median_filter_5.c_str());
- EXPECT_TRUE(chain.configure(10, chain_def));
+ TiXmlElement * config = chain_def.RootElement();
+ EXPECT_TRUE(chain.configure(10, config));
// EXPECT_TRUE(chain.configure(10));
@@ -127,8 +129,10 @@
TiXmlDocument chain_def = TiXmlDocument();
chain_def.Parse(bad_xml.c_str());
- EXPECT_FALSE(chain.configure(5, chain_def));
+ TiXmlElement * config = chain_def.RootElement();
+ EXPECT_FALSE(chain.configure(5, config));
+
}
Modified: pkg/trunk/util/laser_scan/CMakeLists.txt
===================================================================
--- pkg/trunk/util/laser_scan/CMakeLists.txt 2009-02-24 23:31:31 UTC (rev 11692)
+++ pkg/trunk/util/laser_scan/CMakeLists.txt 2009-02-24 23:34:41 UTC (rev 11693)
@@ -8,11 +8,8 @@
rospack_add_library(laser_scan src/laser_scan.cc )
rospack_link_boost(laser_scan thread)
-rospack_add_library(laser_median_filter src/median_filter.cpp )
-rospack_link_boost(laser_median_filter thread)
rospack_add_executable(median_node src/median_filter_node.cpp)
-target_link_libraries(median_node laser_median_filter)
rospack_add_executable(scan_shadows_filter_node src/scan_shadows_filter.cpp)
target_link_libraries (scan_shadows_filter_node laser_scan)
Modified: pkg/trunk/util/laser_scan/include/laser_scan/median_filter.h
===================================================================
--- pkg/trunk/util/laser_scan/include/laser_scan/median_filter.h 2009-02-24 23:31:31 UTC (rev 11692)
+++ pkg/trunk/util/laser_scan/include/laser_scan/median_filter.h 2009-02-24 23:34:41 UTC (rev 11693)
@@ -35,6 +35,7 @@
#include <sstream>
#include "boost/thread/mutex.hpp"
+#include "boost/scoped_ptr.hpp"
#include "laser_scan/LaserScan.h"
#include "filters/median.h"
@@ -45,16 +46,17 @@
namespace laser_scan{
/** \brief A class to provide median filtering of laser scans */
-class LaserMedianFilter
+template <typename T>
+class LaserMedianFilter : public filters::FilterBase<T>
{
public:
/** \brief Constructor
* \param averaging_length How many scans to average over.
*/
- LaserMedianFilter();//const std::string & xml_parameters);
+ LaserMedianFilter();
~LaserMedianFilter();
- bool configure(const std::string & xml);
+ bool configure(unsigned int number_of_channels, TiXmlElement *config); //const std::string & xml);
/** \brief Update the filter and get the response
* \param scan_in The new scan to filter
@@ -73,9 +75,77 @@
filters::FilterChain<std_vector_float > * range_filter_;
filters::FilterChain<std_vector_float > * intensity_filter_;
- std::string latest_xml_;
+ boost::scoped_ptr<TiXmlElement> latest_xml_;
};
+typedef laser_scan::LaserScan laser_scan_laser_scan;
+ROS_REGISTER_FILTER(LaserMedianFilter, laser_scan_laser_scan);
+
+template <typename T>
+LaserMedianFilter<T>::LaserMedianFilter():
+ num_ranges_(1)
+{
+
+};
+
+template <typename T>
+bool LaserMedianFilter<T>::configure(unsigned int number_of_channels, TiXmlElement * xml_doc)
+{
+ ROS_ASSERT(number_of_channels == 1);
+ latest_xml_.reset( xml_doc->Clone()->ToElement());
+
+ range_filter_ = new filters::FilterChain<std::vector<float> >();
+ range_filter_->configure(num_ranges_, latest_xml_.get());
+
+ intensity_filter_ = new filters::FilterChain<std::vector<float> >();
+ intensity_filter_->configure(num_ranges_, latest_xml_.get());
+
+ return true;
+};
+
+template <typename T>
+LaserMedianFilter<T>::~LaserMedianFilter()
+{
+ delete range_filter_;
+ delete intensity_filter_;
+};
+
+template <typename T>
+bool LaserMedianFilter<T>::update(const laser_scan::LaserScan& scan_in, laser_scan::LaserScan& scan_out)
+{
+ boost::mutex::scoped_lock lock(data_lock);
+ scan_out = scan_in; ///Quickly pass through all data \todo don't copy data too
+
+ if (scan_in.get_ranges_size() != num_ranges_) //Reallocating
+ {
+ ROS_INFO("Laser filter clearning and reallocating due to larger scan size");
+ delete range_filter_;
+ delete intensity_filter_;
+
+
+ num_ranges_ = scan_in.get_ranges_size();
+
+
+ range_filter_ = new filters::FilterChain<std::vector<float> >();
+ range_filter_->configure(num_ranges_, latest_xml_.get());
+
+ intensity_filter_ = new filters::FilterChain<std::vector<float> >();
+ intensity_filter_->configure(num_ranges_, latest_xml_.get());
+
+ }
+
+ /** \todo check for length of intensities too */
+ range_filter_->update(scan_in.ranges, scan_out.ranges);
+ intensity_filter_->update(scan_in.intensities, scan_out.intensities);
+
+
+ return true;
}
+
+
+
+}
+
+
#endif //LASER_SCAN_UTILS_LASERSCAN_H
Deleted: pkg/trunk/util/laser_scan/src/median_filter.cpp
===================================================================
--- pkg/trunk/util/laser_scan/src/median_filter.cpp 2009-02-24 23:31:31 UTC (rev 11692)
+++ pkg/trunk/util/laser_scan/src/median_filter.cpp 2009-02-24 23:34:41 UTC (rev 11693)
@@ -1,99 +0,0 @@
-/*
- * Copyright (c) 2008, Willow Garage, Inc.
- * All rights reserved.
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions are met:
- *
- * * Redistributions of source code must retain the above copyright
- * notice, this list of conditions and the following disclaimer.
- * * Redistributions in binary form must reproduce the above copyright
- * notice, this list of conditions and the following disclaimer in the
- * documentation and/or other materials provided with the distribution.
- * * Neither the name of the Willow Garage, Inc. nor the names of its
- * contributors may be used to endorse or promote products derived from
- * this software without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
- * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
- * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
- * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
- * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
- * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
- * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
- * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
- * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
- * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- */
-
-#include "laser_scan/median_filter.h"
-#include <algorithm>
-
-using namespace filters ;
-
-
-namespace laser_scan{
-
-
-LaserMedianFilter::LaserMedianFilter():
- num_ranges_(1)
-{
-
-};
-
-bool LaserMedianFilter::configure(const std::string & xml_parameters)
-{
- latest_xml_ = xml_parameters;
- TiXmlDocument xml_doc;
- xml_doc.Parse(latest_xml_.c_str());
-
- range_filter_ = new FilterChain<std_vector_float >();
- range_filter_->configure(num_ranges_, xml_doc);
-
- intensity_filter_ = new FilterChain<std_vector_float >();
- intensity_filter_->configure(num_ranges_, xml_doc);
-
- return true;
-};
-
-LaserMedianFilter::~LaserMedianFilter()
-{
- delete range_filter_;
- delete intensity_filter_;
-};
-
-bool LaserMedianFilter::update(const laser_scan::LaserScan& scan_in, laser_scan::LaserScan& scan_out)
-{
- boost::mutex::scoped_lock lock(data_lock);
- scan_out = scan_in; ///Quickly pass through all data \todo don't copy data too
-
- if (scan_in.get_ranges_size() != num_ranges_) //Reallocating
- {
- ROS_INFO("Laser filter clearning and reallocating due to larger scan size");
- delete range_filter_;
- delete intensity_filter_;
-
-
- num_ranges_ = scan_in.get_ranges_size();
-
- TiXmlDocument xml_doc;
- xml_doc.Parse(latest_xml_.c_str());
-
- range_filter_ = new FilterChain<std_vector_float >();
- range_filter_->configure(num_ranges_, xml_doc);
-
- intensity_filter_ = new FilterChain<std_vector_float >();
- intensity_filter_->configure(num_ranges_, xml_doc);
-
- }
-
- /** \todo check for length of intensities too */
- range_filter_->update(scan_in.ranges, scan_out.ranges);
- intensity_filter_->update(scan_in.intensities, scan_out.intensities);
-
-
- return true;
-}
-
-}
Modified: pkg/trunk/util/laser_scan/src/median_filter_node.cpp
===================================================================
--- pkg/trunk/util/laser_scan/src/median_filter_node.cpp 2009-02-24 23:31:31 UTC (rev 11692)
+++ pkg/trunk/util/laser_scan/src/median_filter_node.cpp 2009-02-24 23:34:41 UTC (rev 11693)
@@ -46,7 +46,11 @@
node_.advertise<laser_scan::LaserScan>("~output", 1000);
node_.param("~filters", filter_xml, median_filter_xml);
printf("Got ~filters as: %s\n", filter_xml.c_str());
- filter.configure(filter_xml);
+ TiXmlDocument xml_doc;
+ xml_doc.Parse(filter_xml.c_str());
+ TiXmlElement * config = xml_doc.RootElement();
+
+ filter.configure(1, config);
node_.subscribe("scan_in", msg, &MedianFilterNode::callback,this, 3);
}
void callback()
@@ -56,7 +60,7 @@
}
protected:
- laser_scan::LaserMedianFilter filter;
+ laser_scan::LaserMedianFilter<laser_scan::LaserScan> filter;
ros::Node& node_;
};
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|