Menu
▾
▴
[Personalrobots-commit] SF.net SVN: personalrobots:[9174] pkg/trunk
|
From: <tf...@us...> - 2009-01-10 07:58:53
|
Revision: 9174
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=9174&view=rev
Author: tfoote
Date: 2009-01-10 07:58:49 +0000 (Sat, 10 Jan 2009)
Log Message:
-----------
moving shadow and intensity filters into laser_scan package, and decreasing our package count by 2 :-)
Modified Paths:
--------------
pkg/trunk/demos/2dnav_gazebo/2dnav-stack-amcl.xml
pkg/trunk/demos/2dnav_gazebo/2dnav-stack-fake_localization.xml
pkg/trunk/demos/2dnav_gazebo/manifest.xml
pkg/trunk/drivers/robot/pr2/point_cloud_assembler/manifest.xml
pkg/trunk/util/laser_scan/CMakeLists.txt
Added Paths:
-----------
pkg/trunk/util/laser_scan/src/scan_intensity_filter.cpp
pkg/trunk/util/laser_scan/src/scan_shadows_filter.cpp
Removed Paths:
-------------
pkg/trunk/drivers/laser/scan_intensity_filter/
pkg/trunk/drivers/laser/scan_shadows_filter/
Modified: pkg/trunk/demos/2dnav_gazebo/2dnav-stack-amcl.xml
===================================================================
--- pkg/trunk/demos/2dnav_gazebo/2dnav-stack-amcl.xml 2009-01-10 02:59:08 UTC (rev 9173)
+++ pkg/trunk/demos/2dnav_gazebo/2dnav-stack-amcl.xml 2009-01-10 07:58:49 UTC (rev 9174)
@@ -72,7 +72,7 @@
<param name="/scan_shadows_filter/filter_min_angle" value="10"/>
<param name="/scan_shadows_filter/filter_max_angle" value="170"/>
<param name="/scan_shadows_filter/filter_window" value="2"/>
- <node pkg="scan_shadows_filter" type="scan_shadows_filter_node" respawn="false" output="screen" />
+ <node pkg="laser_scan" type="scan_shadows_filter_node" respawn="false" output="screen" />
<!-- move_base_sbpl settings -->
<!-- for the moment use e.g.: ./shparam set move_base/plannerType string ADPlanner -->
Modified: pkg/trunk/demos/2dnav_gazebo/2dnav-stack-fake_localization.xml
===================================================================
--- pkg/trunk/demos/2dnav_gazebo/2dnav-stack-fake_localization.xml 2009-01-10 02:59:08 UTC (rev 9173)
+++ pkg/trunk/demos/2dnav_gazebo/2dnav-stack-fake_localization.xml 2009-01-10 07:58:49 UTC (rev 9174)
@@ -21,7 +21,7 @@
<param name="/scan_shadows_filter/filter_min_angle" value="10"/>
<param name="/scan_shadows_filter/filter_max_angle" value="170"/>
<param name="/scan_shadows_filter/filter_window" value="2"/>
- <node pkg="scan_shadows_filter" type="scan_shadows_filter_node" respawn="false" output="screen" />
+ <node pkg="laser_scan" type="scan_shadows_filter_node" respawn="false" output="screen" />
<!-- wavefront_player settings -->
<!--
Modified: pkg/trunk/demos/2dnav_gazebo/manifest.xml
===================================================================
--- pkg/trunk/demos/2dnav_gazebo/manifest.xml 2009-01-10 02:59:08 UTC (rev 9173)
+++ pkg/trunk/demos/2dnav_gazebo/manifest.xml 2009-01-10 07:58:49 UTC (rev 9174)
@@ -21,7 +21,6 @@
<depend package="pr2_gazebo"/>
<depend package="pr2_prototype1_gazebo"/>
<depend package="transformations"/>
- <depend package="scan_shadows_filter"/>
<depend package="ransac_ground_plane_extraction"/>
<depend package="point_cloud_assembler"/>
</package>
Modified: pkg/trunk/drivers/robot/pr2/point_cloud_assembler/manifest.xml
===================================================================
--- pkg/trunk/drivers/robot/pr2/point_cloud_assembler/manifest.xml 2009-01-10 02:59:08 UTC (rev 9173)
+++ pkg/trunk/drivers/robot/pr2/point_cloud_assembler/manifest.xml 2009-01-10 07:58:49 UTC (rev 9174)
@@ -16,7 +16,6 @@
# For Testing
<depend package="rostest"/>
<depend package="gtest"/>
- <depend package="scan_shadows_filter"/>
<depend package="rosrecord" />
</package>
Modified: pkg/trunk/util/laser_scan/CMakeLists.txt
===================================================================
--- pkg/trunk/util/laser_scan/CMakeLists.txt 2009-01-10 02:59:08 UTC (rev 9173)
+++ pkg/trunk/util/laser_scan/CMakeLists.txt 2009-01-10 07:58:49 UTC (rev 9174)
@@ -5,4 +5,10 @@
rospack_add_library(laser_median_filter src/median_filter.cpp )
rospack_add_executable(median_node src/median_filter_node.cpp)
-target_link_libraries(median_node laser_median_filter)
\ No newline at end of file
+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)
+
+rospack_add_executable(scan_intensity_filter src/scan_intensity_filter.cpp)
+target_link_libraries (scan_intensity_filter laser_scan)
Copied: pkg/trunk/util/laser_scan/src/scan_intensity_filter.cpp (from rev 5430, pkg/trunk/drivers/laser/scan_intensity_filter/scan_intensity_filter.cpp)
===================================================================
--- pkg/trunk/util/laser_scan/src/scan_intensity_filter.cpp (rev 0)
+++ pkg/trunk/util/laser_scan/src/scan_intensity_filter.cpp 2009-01-10 07:58:49 UTC (rev 9174)
@@ -0,0 +1,126 @@
+/*********************************************************************
+* Software License Agreement (BSD License)
+*
+* 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 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.
+*********************************************************************/
+
+
+/**
+
+@mainpage
+
+@htmlinclude manifest.html
+
+\author Vijay Pradeep
+@b ScanIntensityFilter takes input scans and fiters out that are not within the specified range. The filtered out readings are set at >max_range \
+in order to invalidate them.
+
+**/
+
+
+#include "ros/node.h"
+#include "std_msgs/LaserScan.h"
+
+using namespace std_msgs;
+
+class ScanIntensityFilter : public ros::node
+{
+public:
+
+ LaserScan cur_scan_ ;
+ double lower_threshold_ ;
+ double upper_threshold_ ;
+ int disp_hist_ ;
+
+ ScanIntensityFilter() : ros::node("scan_intensity_filter")
+ {
+ advertise<LaserScan>("filtered_scan", 1) ;
+ subscribe("input_scan", cur_scan_, &ScanIntensityFilter::scans_callback, 40) ;
+
+ param("~lower_threshold", lower_threshold_, 8000.0) ;
+ param("~upper_threshold", upper_threshold_, 100000.0) ;
+ param("~disp_histogram", disp_hist_, 1) ;
+ }
+
+ virtual ~ScanIntensityFilter()
+ {
+
+ }
+
+ void scans_callback()
+ {
+ const double hist_max = 4*12000.0 ;
+ const int num_buckets = 24 ;
+ int histogram[num_buckets] ;
+ for (int i=0; i < num_buckets; i++)
+ histogram[i] = 0 ;
+
+ LaserScan filtered_scan ;
+ filtered_scan = cur_scan_ ;
+
+ for (unsigned int i=0; i < cur_scan_.ranges.size(); i++) // Need to check ever reading in the current scan
+ {
+ if (filtered_scan.intensities[i] <= lower_threshold_ || // Is this reading below our lower threshold?
+ filtered_scan.intensities[i] >= upper_threshold_) // Is this reading above our upper threshold?
+ {
+ filtered_scan.ranges[i] = cur_scan_.range_max + 1.0 ; // If so, then make it a value bigger than the max range
+ }
+
+ int cur_bucket = (int) ((filtered_scan.intensities[i]/hist_max)*num_buckets) ;
+ if (cur_bucket >= num_buckets-1)
+ cur_bucket = num_buckets-1 ;
+ histogram[cur_bucket]++ ;
+ }
+
+ if (disp_hist_ > 0) // Display Histogram
+ {
+ printf("********** SCAN **********\n") ;
+ for (int i=0; i < num_buckets; i++)
+ {
+ printf("%u - %u: %u\n", (unsigned int) hist_max/num_buckets*i,
+ (unsigned int) hist_max/num_buckets*(i+1),
+ histogram[i]) ;
+ }
+ }
+
+ publish("filtered_scan", filtered_scan) ; // Publish the filtered data
+ }
+} ;
+
+int main(int argc, char **argv)
+{
+ ros::init(argc, argv) ;
+ ScanIntensityFilter filter ;
+ filter.spin() ;
+ ros::fini() ;
+ return 0 ;
+}
+
Copied: pkg/trunk/util/laser_scan/src/scan_shadows_filter.cpp (from rev 8873, pkg/trunk/drivers/laser/scan_shadows_filter/src/scan_shadows_filter.cpp)
===================================================================
--- pkg/trunk/util/laser_scan/src/scan_shadows_filter.cpp (rev 0)
+++ pkg/trunk/util/laser_scan/src/scan_shadows_filter.cpp 2009-01-10 07:58:49 UTC (rev 9174)
@@ -0,0 +1,258 @@
+/*
+ * Copyright (c) 2008 Radu Bogdan Rusu <ru...@cs...>
+ *
+ * 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.
+ *
+ * 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.
+ *
+ * $Id: scan_shadows_filter.cpp,v 1.0 2008/12/04 12:00:00 rusu Exp $
+ *
+ */
+
+/*
+\author Radu Bogdan Rusu <ru...@cs...>
+
+
+ */
+
+#include <ros/node.h>
+#include <std_msgs/PointCloud.h>
+#include <std_msgs/LaserScan.h>
+
+#include <float.h>
+
+// Laser projection
+#include <laser_scan/laser_scan.h>
+
+using namespace std_msgs;
+
+/** @b ScanShadowsFilter is a simple node that filters shadow points in a laser scan line and publishes the results in a cloud.
+ */
+class ScanShadowsFilter : public ros::node
+{
+ public:
+
+ // ROS related
+ LaserScan tilt_scan_msg_; // Filled by subscriber with new tilte laser scans
+ laser_scan::LaserProjection projector_; // Used to project laser scans
+
+ double tilt_laser_max_range_; // Used in laser scan projection
+ double min_angle_, max_angle_; // Filter angle threshold
+ int window_;
+
+ ////////////////////////////////////////////////////////////////////////////////
+ ScanShadowsFilter () : ros::node ("scan_shadows_filter"), tilt_laser_max_range_ (DBL_MAX)
+ {
+ subscribe("tilt_scan", tilt_scan_msg_, &ScanShadowsFilter::tiltScanCallback, 10);
+
+ advertise<PointCloud> ("tilt_laser_cloud_filtered", 10);
+
+ param ("~filter_min_angle", min_angle_, 10.0);
+ param ("~filter_max_angle", max_angle_, 170.0);
+ param ("~filter_window", window_, 2);
+ }
+
+ ////////////////////////////////////////////////////////////////////////////////
+ virtual ~ScanShadowsFilter () { }
+
+ ////////////////////////////////////////////////////////////////////////////////
+ /**
+ * \brief Computes the angle between the two lines created from 2 points and the
+ * viewpoint. Returns the angle (in degrees).
+ * \param px X coordinate for the first point
+ * \param py Y coordinate for the first point
+ * \param pz Z coordinate for the first point
+ * \param qx X coordinate for the second point
+ * \param qy Y coordinate for the second point
+ * \param qz Z coordinate for the second point
+ */
+ inline double
+ getAngleWithViewpoint (float px, float py, float pz, float qx, float qy, float qz)
+ {
+ double dir_a[3], dir_b[3];
+ dir_a[0] = - px; dir_a[1] = - py; dir_a[2] = - pz; // Assume viewpoint is 0,0,0
+ dir_b[0] = qx - px; dir_b[1] = qy - py; dir_b[2] = qz - pz;
+
+ // sqrt (sqr (x) + sqr (y) + sqr (z))
+ double norm_a = sqrt (dir_a[0]*dir_a[0] + dir_a[1]*dir_a[1] + dir_a[2]*dir_a[2]);
+ // Check for bogus 0,0,0 points
+ if (norm_a == 0) return (0);
+ double norm_b = sqrt (dir_b[0]*dir_b[0] + dir_b[1]*dir_b[1] + dir_b[2]*dir_b[2]);
+ if (norm_b == 0) return (0);
+ // dot_product (x, y)
+ double dot_pr = dir_a[0]*dir_b[0] + dir_a[1]*dir_b[1] + dir_a[2]*dir_b[2];
+ if (dot_pr != dot_pr) // Check for NaNs
+ return (0);
+
+ return ( acos (dot_pr / (norm_a * norm_b) ) * 180.0 / M_PI);
+ }
+
+ ////////////////////////////////////////////////////////////////////////////////
+ /** \brief Given a PointCloud representing a single laser scan (usually obtained
+ * after LaserProjection's projectLaser(), and the index of the channel
+ * representing the true measurement "index", create a complete PointCloud
+ * representation which replaces the invalid measurements with 0 values.
+ * \param c_idx the channel index for the "index"
+ * \param cloud_in the input PointCloud message
+ * \param cloud_out the output PointCloud message
+ */
+ void
+ constructCompleteLaserScanCloud (int c_idx, PointCloud cloud_in, PointCloud &cloud_out)
+ {
+ // Use the index to revert to a full laser scan cloud (inefficient locally, but efficient globally)
+ int idx = 0;
+ for (unsigned int i = 0; i < cloud_out.pts.size (); i++)
+ {
+ unsigned int j = (int)cloud_in.chan[c_idx].vals[idx]; // Find out the true index value
+ if (i == j)
+ {
+ // Copy relevant data
+ cloud_out.pts[i].x = cloud_in.pts[idx].x;
+ cloud_out.pts[i].y = cloud_in.pts[idx].y;
+ cloud_out.pts[i].z = cloud_in.pts[idx].z;
+ for (unsigned int d = 0; d < cloud_out.get_chan_size (); d++)
+ cloud_out.chan[d].vals[i] = cloud_in.chan[d].vals[idx];
+
+ idx++; // Assume chan['index'] is sorted (which should be true)
+ if (idx >= (int)cloud_in.chan[c_idx].vals.size ()) idx = cloud_in.chan[c_idx].vals.size () - 1;
+ }
+ else
+ {
+ // Bogus XYZ entry. No need to copy channels.
+ cloud_out.pts[i].x = cloud_out.pts[i].y = cloud_out.pts[i].z = 0;
+ }
+ }
+ }
+
+ ////////////////////////////////////////////////////////////////////////////////
+ /** \brief Filter shadow points based on 3 global parameters: min_angle, max_angle
+ * and window. {min,max}_angle specify the allowed angle interval (in degrees)
+ * between the created lines (see getAngleWithViewPoint). Window specifies how many
+ * consecutive measurements to take into account for one point.
+ * \param cloud_in the input PointCloud message
+ * \param cloud_out the output PointCloud message
+ */
+ void
+ filterShadowPoints (PointCloud cloud_in, PointCloud &cloud_out)
+ {
+ // For each point in the current line scan
+ int n_pts_filtered = 0;
+ for (unsigned int i = 0; i < cloud_in.pts.size (); i++)
+ {
+ bool valid_point = true;
+ for (int y = -window_; y < window_ + 1; y++)
+ {
+ int j = i + y;
+ if ( j < 0 || j >= (int)cloud_in.pts.size () || (int)i == j ) // Out of scan bounds or itself
+ continue;
+
+ double angle = getAngleWithViewpoint (cloud_in.pts[i].x, cloud_in.pts[i].y, cloud_in.pts[i].z,
+ cloud_in.pts[j].x, cloud_in.pts[j].y, cloud_in.pts[j].z);
+ if (angle < min_angle_ || angle > max_angle_)
+ valid_point = false;
+ }
+
+ // If point found as 'ok', copy the relevant data
+ if (valid_point)
+ {
+ cloud_out.pts[n_pts_filtered].x = cloud_in.pts[i].x;
+ cloud_out.pts[n_pts_filtered].y = cloud_in.pts[i].y;
+ cloud_out.pts[n_pts_filtered].z = cloud_in.pts[i].z;
+
+ for (unsigned int d = 0; d < cloud_out.get_chan_size (); d++)
+ cloud_out.chan[d].vals[n_pts_filtered] = cloud_in.chan[d].vals[i];
+
+ n_pts_filtered++;
+ }
+ }
+
+ // Resize output vectors
+ cloud_out.pts.resize (n_pts_filtered);
+ for (unsigned int d = 0; d < cloud_out.get_chan_size (); d++)
+ cloud_out.chan[d].vals.resize (n_pts_filtered);
+ }
+
+ ////////////////////////////////////////////////////////////////////////////////
+ void
+ tiltScanCallback ()
+ {
+ // Project laser into point cloud
+ PointCloud tilt_cloud;
+ int n_scan = tilt_scan_msg_.ranges.size (); // Save the number of measurements
+
+ // Transform into a PointCloud message
+ int mask = laser_scan::MASK_INTENSITY + laser_scan::MASK_DISTANCE + laser_scan::MASK_INDEX;
+ projector_.projectLaser (tilt_scan_msg_, tilt_cloud, tilt_laser_max_range_, false, mask);//, true);
+
+ /// ---[ Perhaps unnecessary, but find out which channel contains the index
+ int c_idx = -1;
+ for (unsigned int d = 0; d < tilt_cloud.get_chan_size (); d++)
+ {
+ if (tilt_cloud.chan[d].name == "index")
+ {
+ c_idx = d;
+ break;
+ }
+ }
+ if (c_idx == -1 || tilt_cloud.chan[c_idx].vals.size () == 0) return;
+ /// ]--
+
+ // Prepare the storage for the temporary array ([] and resize are faster than push_back)
+ PointCloud tilt_full_cloud (tilt_cloud);
+ tilt_full_cloud.pts.resize (n_scan);
+ for (unsigned int d = 0; d < tilt_cloud.get_chan_size (); d++)
+ tilt_full_cloud.chan[d].vals.resize (n_scan);
+
+ // Prepare data storage for the output array ([] and resize are faster than push_back)
+ PointCloud filtered_cloud (tilt_cloud);
+ filtered_cloud.pts.resize (n_scan);
+ for (unsigned int d = 0; d < tilt_cloud.get_chan_size (); d++)
+ filtered_cloud.chan[d].vals.resize (n_scan);
+
+ // Construct a complete laser cloud resembling the original LaserScan (0..LASER_MAX measurements)
+ constructCompleteLaserScanCloud (c_idx, tilt_cloud, tilt_full_cloud);
+
+ // Filter points
+ filterShadowPoints (tilt_full_cloud, filtered_cloud);
+
+ // Set timestamp/frameid and publish
+ filtered_cloud.header = tilt_scan_msg_.header;
+ publish ("tilt_laser_cloud_filtered", filtered_cloud);
+ }
+
+} ;
+
+/* ---[ */
+int
+ main (int argc, char** argv)
+{
+ ros::init (argc, argv);
+
+ ScanShadowsFilter f;
+ f.spin ();
+
+ ros::fini ();
+
+ return (0);
+}
+/* ]--- */
+
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
