Initial commit

.svn/entries

+12

.svn/format

+12

.svn/pristine/14/14939b7bc3e235246fcf3fa7f52ff827636a4e79.svn-base

+#include "obstacle_detection_normals_alg.h"
+using namespace std;
+
+ObstacleDetectionNormalsAlgorithm::ObstacleDetectionNormalsAlgorithm(void)
+{
+}
+
+ObstacleDetectionNormalsAlgorithm::~ObstacleDetectionNormalsAlgorithm(void)
+{
+}
+
+void ObstacleDetectionNormalsAlgorithm::config_update(Config& new_cfg, uint32_t level)
+{
+  this->lock();
+
+  // save the current configuration
+  this->config_=new_cfg;
+  
+  this->unlock();
+}
+
+// ObstacleDetectionNormalsAlgorithm Public API
+
+void ObstacleDetectionNormalsAlgorithm::cloud_all(int KSearch, float normal_z, 
+     float normal_y, float normal_x, float min_dist, 
+     const pcl::PointCloud<pcl::PointXYZ>& cloud, 
+     pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr& cloud_out, pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr& cloud_obs)
+{
+
+//pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ>);
+  // Output datasets
+  pcl::PointCloud<pcl::Normal>::Ptr output (new pcl::PointCloud<pcl::Normal>);
+
+// Normal estimation using integral Images /////////////////////////////////////////////
+// NOTE: Point cloud should contain width and height structure
+
+// pcl::IntegralImageNormalEstimation<pcl::PointXYZ, pcl::Normal> n;
+
+// n.setNormalEstimationMethod(n.AVERAGE_3D_GRADIENT);
+// n.setMaxDepthChangeFactor(0.02f);
+// n.setNormalSmoothingSize(10.0f);
+// n.setInputCloud(cloud);
+// n.compute(*output);
+
+// ROS Fuerte version //////////////////////////////////////////////////////////////
+
+  // Create the normal estimation class, and pass the input dataset to it
+  pcl::NormalEstimation<pcl::PointXYZ, pcl::Normal> n;
+  // [Multi-threat](NOTE: Point cloud should contain width and height structure):
+  // pcl::NormaleEstimationOMP<pcl::PointXYZ, pcl::Normal> n;
+  // n.setNumberOfThreads(8); 
+
+  // To work with Pointers ::Ptr ///////////////////////////////////////////////////////////
+  // // Set input cloud
+  // n.setInputCloud (cloud);
+            
+  // // Create an empty kdtree representation, and pass it to the normal estimation object.
+  // // Its content will be filled inside the object, based on the given input dataset (as no other search surface is given).
+  // pcl::search::KdTree<pcl::PointXYZ>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZ> ());
+  // n.setSearchMethod (tree);
+
+  // // Use all neighbors in a sphere of radius 3cm
+  // n.setRadiusSearch (KSearch);
+  // // [Multi-threat](NOTE: Point cloud should contain width and height structure) :
+  // // n.setKSearch (KSearch);
+
+  // double secs = ros::Time::now().toSec();
+
+  // // Compute the features
+  // n.compute (*output);
+
+  // pcl::concatenateFields (*cloud, *output, *cloud_out);
+
+  // double secs2 = ros::Time::now().toSec();
+  // double diff = secs2-secs;
+  // ROS_INFO("computation time: %2.2f",diff);
+
+/////////////////////////////////////////////////////////////////////////////////
+
+  std::vector<int> indices;
+  indices.resize (cloud.points.size());
+  for (size_t i = 0; i < indices.size (); ++i) { indices[i] = i; }
+
+  pcl::search::KdTree<pcl::PointXYZ>::Ptr tree(new pcl::search::KdTree<pcl::PointXYZ>());
+
+
+  boost::shared_ptr< vector<int> > indicesptr(new vector<int> (indices));
+  // set normal estimation parameters
+  n.setIndices(indicesptr);
+  n.setSearchMethod(tree);
+  n.setKSearch(KSearch); // Use 10 nearest neighbors to estimate the normals
+
+  //estimate
+  n.setInputCloud(cloud.makeShared());
+  n.compute(*output);
+
+  pcl::concatenateFields (cloud, *output, *cloud_out);
+
+
+
+  float e=0.0;
+
+  *cloud_obs=*cloud_out; 
+
+  for (size_t rowIndex=0, pointIndex=0; rowIndex<cloud_out->height; ++rowIndex)
+  {
+    for (size_t colIndex=0; colIndex<cloud_out->width; ++colIndex, ++pointIndex)
+    {
+      //ROS_ERROR_STREAM(cloud_out->points[pointIndex].normal_z);
+      if (cloud_out->points[pointIndex].z > min_dist)
+      {
+        if (cloud_out->points[pointIndex].normal_z < normal_z && 
+            cloud_out->points[pointIndex].normal_y < normal_y && 
+            cloud_out->points[pointIndex].normal_x < normal_x)
+        {
+          cloud_out->points[pointIndex].r=0;
+          cloud_out->points[pointIndex].g=255;
+          cloud_out->points[pointIndex].b=0;
+          cloud_obs->points[pointIndex].x=std::numeric_limits<float>::quiet_NaN();;
+          cloud_obs->points[pointIndex].y=std::numeric_limits<float>::quiet_NaN();;
+          cloud_obs->points[pointIndex].z=std::numeric_limits<float>::quiet_NaN();;
+        }
+        else 
+        {
+          cloud_out->points[pointIndex].r=255;
+          cloud_out->points[pointIndex].g=0;
+          cloud_out->points[pointIndex].b=0;
+          //error counter looking a plane without obstacles
+          e=e+1.0;
+        }
+      }
+      else
+      {
+        cloud_out->points[pointIndex].r=0;
+        cloud_out->points[pointIndex].g=0;
+        cloud_out->points[pointIndex].b=255;
+      }
+    }
+  }
+  e=(e/float(cloud_out->width*cloud_out->height))*100; 
+  //printf("error %f",e); 
+}
+

.svn/pristine/18/1889bdbc30453ce7ddc34829b6d785dfe6c21f04.svn-base

+/**
+\mainpage
+\htmlinclude manifest.html
+
+\b iri_obstacle_detection_normals is ... 
+
+<!-- 
+Provide an overview of your package.
+-->
+
+
+\section codeapi Code API
+
+<!--
+Provide links to specific auto-generated API documentation within your
+package that is of particular interest to a reader. Doxygen will
+document pretty much every part of your code, so do your best here to
+point the reader to the actual API.
+
+If your codebase is fairly large or has different sets of APIs, you
+should use the doxygen 'group' tag to keep these APIs together. For
+example, the roscpp documentation has 'libros' group.
+-->
+
+
+*/

.svn/pristine/28/28647fe131daa3e90296c971ffa0f5a8fdd816de.svn-base

+// Copyright (C) 2010-2011 Institut de Robotica i Informatica Industrial, CSIC-UPC.
+// Author 
+// All rights reserved.
+//
+// This file is part of iri-ros-pkg
+// iri-ros-pkg is free software: you can redistribute it and/or modify
+// it under the terms of the GNU Lesser General Public License as published by
+// the Free Software Foundation, either version 3 of the License, or
+// at your option) any later version.
+//
+// This program is distributed in the hope that it will be useful,
+// but WITHOUT ANY WARRANTY; without even the implied warranty of
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+// GNU Lesser General Public License for more details.
+//
+// You should have received a copy of the GNU Lesser General Public License
+// along with this program.  If not, see <http://www.gnu.org/licenses/>.
+// 
+// IMPORTANT NOTE: This code has been generated through a script from the 
+// iri_ros_scripts. Please do NOT delete any comments to guarantee the correctness
+// of the scripts. ROS topics can be easly add by using those scripts. Please
+// refer to the IRI wiki page for more information:
+// http://wikiri.upc.es/index.php/Robotics_Lab
+
+#ifndef _obstacle_detection_normals_alg_h_
+#define _obstacle_detection_normals_alg_h_
+
+#include <iri_obstacle_detection_normals/ObstacleDetectionNormalsConfig.h>
+#include "mutex.h"
+
+
+#include <pcl/point_types.h>
+#include <pcl/io/pcd_io.h>
+#include <pcl/features/normal_3d.h>
+#include <pcl/features/integral_image_normal.h>
+#include <pcl/kdtree/kdtree_flann.h>
+#include <pcl/impl/point_types.hpp>
+
+#include <boost/make_shared.hpp>
+
+using namespace pcl;
+using namespace std;
+
+typedef KdTree<PointXYZ>::Ptr KdTreePtr;
+
+//include obstacle_detection_normals_alg main library
+
+/**
+ * \brief IRI ROS Specific Driver Class
+ *
+ *
+ */
+class ObstacleDetectionNormalsAlgorithm
+{
+  protected:
+   /**
+    * \brief define config type
+    *
+    * Define a Config type with the ObstacleDetectionNormalsConfig. All driver implementations
+    * will then use the same variable type Config.
+    */
+    CMutex alg_mutex_;
+
+    // private attributes and methods
+    int KSearch;
+    float normal_z,normal_y,normal_x,min_dist;
+
+    KdTreePtr tree;
+    
+    // pcl::PointCloud<pcl::PointXYZ> cloud;
+    // pcl::PointCloud<pcl::PointXYZRGBNormal> cloud_out;
+    // pcl::PointCloud<pcl::PointXYZRGBNormal> cloud_obs;  
+
+
+  public:
+   /**
+    * \brief define config type
+    *
+    * Define a Config type with the ObstacleDetectionNormalsConfig. All driver implementations
+    * will then use the same variable type Config.
+    */
+    typedef iri_obstacle_detection_normals::ObstacleDetectionNormalsConfig Config;
+
+   /**
+    * \brief config variable
+    *
+    * This variable has all the driver parameters defined in the cfg config file.
+    * Is updated everytime function config_update() is called.
+    */
+    Config config_;
+
+   /**
+    * \brief constructor
+    *
+    * In this constructor parameters related to the specific driver can be
+    * initalized. Those parameters can be also set in the openDriver() function.
+    * Attributes from the main node driver class IriBaseDriver such as loop_rate,
+    * may be also overload here.
+    */
+    ObstacleDetectionNormalsAlgorithm(void);
+
+   /**
+    * \brief Lock Algorithm
+    *
+    * Locks access to the Algorithm class
+    */
+    void lock(void) { alg_mutex_.enter(); };
+
+   /**
+    * \brief Unlock Algorithm
+    *
+    * Unlocks access to the Algorithm class
+    */
+    void unlock(void) { alg_mutex_.exit(); };
+
+   /**
+    * \brief Tries Access to Algorithm
+    *
+    * Tries access to Algorithm
+    * 
+    * \return true if the lock was adquired, false otherwise
+    */
+    bool try_enter(void) { return alg_mutex_.try_enter(); };
+
+   /**
+    * \brief config update
+    *
+    * In this function the driver parameters must be updated with the input
+    * config variable. Then the new configuration state will be stored in the 
+    * Config attribute.
+    *
+    * \param new_cfg the new driver configuration state
+    *
+    * \param level level in which the update is taken place
+    */
+    void config_update(Config& new_cfg, uint32_t level=0);
+
+    // here define all obstacle_detection_normals_alg interface methods to retrieve and set
+    // the driver parameters
+
+   /**
+    * \brief Destructor
+    *
+    * This destructor is called when the object is about to be destroyed.
+    *
+    */
+    ~ObstacleDetectionNormalsAlgorithm(void);
+    
+    // void cloud_all(int KSearch, float normal_z, 
+    //  float normal_y, float normal_x, float min_intens, float min_dist, 
+    //  const pcl::PointCloud<pcl::PointXYZ>& cloud, 
+    //  pcl::PointCloud<pcl::PointXYZRGBNormal>& cloud_out, pcl::PointCloud<pcl::PointXYZRGBNormal>& cloud_obs);   
+
+    void cloud_all(int KSearch, float normal_z, 
+     float normal_y, float normal_x, float min_dist, 
+     const pcl::PointCloud<pcl::PointXYZ>& cloud, 
+     pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr& cloud_out, pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr& cloud_obs); 
+
+};
+
+#endif

.svn/pristine/32/324076a9b2f56b1de51ca012ba0f04a96ab540de.svn-base

+\subsubsection parameters ROS parameters
+
+Reads and maintains the following parameters on the ROS server
+
+- \b "~filter_min_dist" : \b [double] Min distance threshold min: 0.0, default: 0.3, max: 100.0
+- \b "~normal_KSearch" : \b [int] No of neighbors to estimate the surface min: 10, default: 100, max: 200
+- \b "~normal_x" : \b [double] Accepted Normal X component min: -1.0, default: 1.0, max: 1.0
+- \b "~normal_y" : \b [double] Accepted Normal Y component min: -1.0, default: -0.2, max: 1.0
+- \b "~normal_z" : \b [double] Accepted Normal Z component min: -1.0, default: 1.0, max: 1.0
+

.svn/pristine/60/60b100ad44ac66b98c4ec45e97cbde679e94d06b.svn-base

+\subsubsection usage Usage
+\verbatim
+<node name="ObstacleDetectionNormalsAlgorithm" pkg="iri_obstacle_detection_normals" type="ObstacleDetectionNormalsAlgorithm">
+  <param name="filter_min_dist" type="double" value="0.3" />
+  <param name="normal_KSearch" type="int" value="100" />
+  <param name="normal_x" type="double" value="1.0" />
+  <param name="normal_y" type="double" value="-0.2" />
+  <param name="normal_z" type="double" value="1.0" />
+</node>
+\endverbatim
+

.svn/pristine/64/6458d6d6edbaeb05315cb76e0ddd8ff569d68937.svn-base

+cmake_minimum_required(VERSION 2.4.6)
+include($ENV{ROS_ROOT}/core/rosbuild/rosbuild.cmake)
+
+set(PROJECT_NAME obstacle_detection_normals_alg_node)
+
+# Set the build type.  Options are:
+#  Coverage       : w/ debug symbols, w/o optimization, w/ code-coverage
+#  Debug          : w/ debug symbols, w/o optimization
+#  Release        : w/o debug symbols, w/ optimization
+#  RelWithDebInfo : w/ debug symbols, w/ optimization
+#  MinSizeRel     : w/o debug symbols, w/ optimization, stripped binaries
+#set(ROS_BUILD_TYPE RelWithDebInfo)
+
+rosbuild_init()
+
+#set the default path for built executables to the "bin" directory
+set(EXECUTABLE_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/bin)
+#set the default path for built libraries to the "lib" directory
+set(LIBRARY_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/lib)
+
+#uncomment if you have defined messages
+#rosbuild_genmsg()
+#uncomment if you have defined services
+#rosbuild_gensrv()
+
+# added to include support for dynamic reconfiguration
+rosbuild_find_ros_package(dynamic_reconfigure)
+include(${dynamic_reconfigure_PACKAGE_PATH}/cmake/cfgbuild.cmake)
+gencfg()
+# end dynamic reconfiguration
+
+FIND_PACKAGE(iriutils REQUIRED)
+FIND_PACKAGE(PCL 1.4 REQUIRED) 
+
+INCLUDE_DIRECTORIES(${iriutils_INCLUDE_DIR} ./include ${PCL_INCLUDE_DIRS})
+link_directories(${PCL_LIBRARY_DIRS})
+add_definitions(${PCL_DEFINITIONS}) 
+
+#common commands for building c++ executables and libraries
+#rosbuild_add_library(${PROJECT_NAME} src/example.cpp)
+#target_link_libraries(${PROJECT_NAME} another_library)
+#rosbuild_add_boost_directories()
+#rosbuild_link_boost(${PROJECT_NAME} thread)
+rosbuild_add_executable(${PROJECT_NAME} src/obstacle_detection_normals_alg.cpp src/obstacle_detection_normals_alg_node.cpp)
+
+target_link_libraries(${PROJECT_NAME} ${iriutils_LIBRARY} ${PCL_LIBRARIES})

.svn/pristine/6c/6cfa2ca439db29dfe2a0f512c0850ab2c7dfafec.svn-base

+# Autogenerated param section. Do not hand edit.
+param {
+group.0 {
+name=Dynamically Reconfigurable Parameters
+desc=See the [[dynamic_reconfigure]] package for details on dynamically reconfigurable parameters.
+0.name= ~filter_min_dist
+0.default= 0.3
+0.type= double
+0.desc=Min distance threshold Range: 0.0 to 100.0
+1.name= ~normal_KSearch
+1.default= 100
+1.type= int
+1.desc=No of neighbors to estimate the surface Range: 10 to 200
+2.name= ~normal_x
+2.default= 1.0
+2.type= double
+2.desc=Accepted Normal X component Range: -1.0 to 1.0
+3.name= ~normal_y
+3.default= -0.2
+3.type= double
+3.desc=Accepted Normal Y component Range: -1.0 to 1.0
+4.name= ~normal_z
+4.default= 1.0
+4.type= double
+4.desc=Accepted Normal Z component Range: -1.0 to 1.0
+}
+}
+# End of autogenerated section. You may edit below.

.svn/pristine/71/71bcccd2efe711e112f0e4b8e1c2465a86133a6d.svn-base

+include $(shell rospack find mk)/cmake.mk
\ No newline at end of file

.svn/pristine/81/8185ede5ce9a8aa852cc3e7d55be131967febd62.svn-base

+#! /usr/bin/env python
+#*  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.
+#***********************************************************
+
+# Author: 
+
+PACKAGE='iri_obstacle_detection_normals'
+import roslib; roslib.load_manifest(PACKAGE)
+
+from dynamic_reconfigure.parameter_generator import *
+
+gen = ParameterGenerator()
+
+#       Name                       Type       Reconfiguration level            Description                       Default   Min   Max
+#gen.add("velocity_scale_factor",  double_t,  0,                               "Maximum velocity scale factor",  0.5,      0.0,  1.0)
+gen.add(       "filter_min_dist",  double_t,  0,                                      "Min distance threshold",  0.3,        0,  100)
+gen.add(        "normal_KSearch",     int_t,  0,                     "No of neighbors to estimate the surface",  100,       10,  200)
+gen.add(              "normal_x",  double_t,  0, 														  	 "Accepted Normal X component",    1,       -1,    1)
+gen.add(              "normal_y",  double_t,  0, 															   "Accepted Normal Y component", -0.2,       -1,    1)
+gen.add(              "normal_z",  double_t,  0, 															   "Accepted Normal Z component",    1,       -1,    1)
+exit(gen.generate(PACKAGE, "ObstacleDetectionNormalsAlgorithm", "ObstacleDetectionNormals"))

.svn/pristine/90/906e2a3ef295d209c3e7057a176bd4310937663f.svn-base

+#include "obstacle_detection_normals_alg_node.h"
+
+ObstacleDetectionNormalsAlgNode::ObstacleDetectionNormalsAlgNode(void) :
+  algorithm_base::IriBaseAlgorithm<ObstacleDetectionNormalsAlgorithm>(),KSearch(100),normal_z(1),
+  normal_y(-0.2),normal_x(1),min_dist(0.3)
+{
+  //init class attributes if necessary
+  //this->loop_rate_ = 2;//in [Hz]
+
+  // [init publishers]
+  this->obstacles_publisher_ = this->public_node_handle_.advertise<sensor_msgs::PointCloud>("obstacles", 1);
+  this->all_rg_publisher_ = this->public_node_handle_.advertise<sensor_msgs::PointCloud2>("all_rg", 1);
+  
+  // [init subscribers]
+  this->input_subscriber_ = this->public_node_handle_.subscribe("input", 1, &ObstacleDetectionNormalsAlgNode::input_callback, this);
+  
+  // [init services]
+  
+  // [init clients]
+  
+  // [init action servers]
+  
+  // [init action clients]
+}
+
+ObstacleDetectionNormalsAlgNode::~ObstacleDetectionNormalsAlgNode(void)
+{
+  // [free dynamic memory]
+}
+
+void ObstacleDetectionNormalsAlgNode::mainNodeThread(void)
+{
+  // [fill msg structures]
+  //this->PointCloud2_msg_.data = my_var;
+  
+  // [fill srv structure and make request to the server]
+  
+  // [fill action structure and make request to the action server]
+
+  // [publish messages]
+
+}
+
+/*  [subscriber callbacks] */
+void ObstacleDetectionNormalsAlgNode::input_callback(const sensor_msgs::PointCloud2::ConstPtr& msg) 
+{ 
+  //ROS_INFO("ObstacleDetectionNormalsAlgNode::input_callback: New Message Received"); 
+
+  // Convert the sensor_msgs/PointCloud2 data to pcl/PointCloud
+  pcl::PointCloud<pcl::PointXYZ> cloud;
+
+  // pcl::PointCloud<pcl::PointXYZ> cloud(new pcl::PointCloud<pcl::PointXYZ>());
+  pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr cloud_out(new pcl::PointCloud<pcl::PointXYZRGBNormal>());
+  pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr cloud_obs(new pcl::PointCloud<pcl::PointXYZRGBNormal>());
+
+  pcl::fromROSMsg (*msg, cloud);
+
+  // pcl::PointCloud<pcl::PointXYZ> cloud_in;
+  // std::vector<int> indices;
+  // pcl::removeNaNFromPointCloud (cloud, cloud_in, indices);
+  
+////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+  //use appropiate mutex to shared variables if necessary 
+  this->alg_.lock(); 
+
+  if (!cloud.empty()){
+    obstacle_detect.cloud_all(KSearch, normal_z, normal_y, normal_x, min_dist, cloud, cloud_out, cloud_obs);
+  }
+  //unlock previously blocked shared variables 
+  this->alg_.unlock();
+
+//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+  // all_rg_msg_.width=msg->width;
+  // all_rg_msg_.height=msg->height;
+  // all_rg_msg_.is_dense=msg->is_dense;
+
+  // obstacles_msg_.header=msg->header;
+  // obstacles_msg_.width=msg->width;
+  // obstacles_msg_.height=msg->height;
+  // obstacles_msg_.is_dense=msg->is_dense;
+
+	pcl::toROSMsg (*cloud_out, all_rg_msg_);
+	pcl::toROSMsg (*cloud_obs, obstacles_msg_);
+
+  sensor_msgs::convertPointCloud2ToPointCloud(obstacles_msg_, obstaclespc_msg_);
+  
+  all_rg_msg_.header=msg->header;
+
+  this->obstacles_publisher_.publish(this->obstaclespc_msg_);
+  this->all_rg_publisher_.publish(this->all_rg_msg_);
+
+  
+  //use appropiate mutex to shared variables if necessary 
+  //this->alg_.lock(); 
+  //this->input_mutex_.enter(); 
+
+  //std::cout << msg->data << std::endl; 
+
+  //unlock previously blocked shared variables 
+  //this->alg_.unlock(); 
+  //this->input_mutex_.exit(); 
+}
+
+/*  [service callbacks] */
+
+/*  [action callbacks] */
+
+/*  [action requests] */
+
+void ObstacleDetectionNormalsAlgNode::node_config_update(Config &config, uint32_t level)
+{
+  this->alg_.lock();
+min_dist=config.filter_min_dist;
+KSearch=config.normal_KSearch;
+normal_z=config.normal_z;
+normal_y=config.normal_y;
+normal_x=config.normal_x;
+  this->alg_.unlock();
+}
+
+void ObstacleDetectionNormalsAlgNode::addNodeDiagnostics(void)
+{
+}
+
+
+/* main function */
+int main(int argc,char *argv[])
+{
+  return algorithm_base::main<ObstacleDetectionNormalsAlgNode>(argc, argv, "obstacle_detection_normals_alg_node");
+}

.svn/pristine/ad/adcb75c525ca6d00273fb0d373b50c172dab4b9c.svn-base

+<package>
+  <description brief="iri_obstacle_detection_normals">
+
+     iri_obstacle_detection_normals
+
+  </description>
+  <author>asantamaria</author>
+  <license>LGPL</license>
+  <review status="unreviewed" notes=""/>
+  <url>http://ros.org/wiki/iri_obstacle_detection_normals</url>
+  <depend package="roscpp"/>
+  <depend package="iri_base_algorithm"/>
+  <depend package="sensor_msgs"/>
+  <depend package="pcl_ros"/>
+</package>
+
+

.svn/pristine/ed/ed15466727c1a5ecc8d3c57171097494a531d9ca.svn-base

+// Copyright (C) 2010-2011 Institut de Robotica i Informatica Industrial, CSIC-UPC.
+// Author 
+// All rights reserved.
+//
+// This file is part of iri-ros-pkg
+// iri-ros-pkg is free software: you can redistribute it and/or modify
+// it under the terms of the GNU Lesser General Public License as published by
+// the Free Software Foundation, either version 3 of the License, or
+// at your option) any later version.
+//
+// This program is distributed in the hope that it will be useful,
+// but WITHOUT ANY WARRANTY; without even the implied warranty of
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+// GNU Lesser General Public License for more details.
+//
+// You should have received a copy of the GNU Lesser General Public License
+// along with this program.  If not, see <http://www.gnu.org/licenses/>.
+// 
+// IMPORTANT NOTE: This code has been generated through a script from the 
+// iri_ros_scripts. Please do NOT delete any comments to guarantee the correctness
+// of the scripts. ROS topics can be easly add by using those scripts. Please
+// refer to the IRI wiki page for more information:
+// http://wikiri.upc.es/index.php/Robotics_Lab
+
+#ifndef _obstacle_detection_normals_alg_node_h_
+#define _obstacle_detection_normals_alg_node_h_
+
+#include <iri_base_algorithm/iri_base_algorithm.h>
+#include "obstacle_detection_normals_alg.h"
+
+#include <ros/ros.h>
+
+#include <pcl_ros/point_cloud.h>
+#include <pcl/ros/conversions.h>
+#include <pcl/point_types.h>
+#include <pcl/filters/filter.h>
+
+
+// [publisher subscriber headers]
+#include <sensor_msgs/PointCloud2.h>
+#include <sensor_msgs/PointCloud.h>
+#include <sensor_msgs/point_cloud_conversion.h>
+// [service client headers]
+
+// [action server client headers]
+
+using namespace std;
+
+
+/**
+ * \brief IRI ROS Specific Algorithm Class
+ *
+ */
+class ObstacleDetectionNormalsAlgNode : public algorithm_base::IriBaseAlgorithm<ObstacleDetectionNormalsAlgorithm>
+{
+  private:
+
+    int KSearch;
+    float normal_z,normal_y,normal_x,min_dist;
+
+    // [publisher attributes]
+    ros::Publisher obstacles_publisher_;
+    ros::Publisher all_rg_publisher_;
+    sensor_msgs::PointCloud2 obstacles_msg_;
+    sensor_msgs::PointCloud obstaclespc_msg_;
+    sensor_msgs::PointCloud2 all_rg_msg_;
+
+    
+
+    // [subscriber attributes]
+    ros::Subscriber input_subscriber_;
+    void input_callback(const sensor_msgs::PointCloud2::ConstPtr& msg);
+    CMutex input_mutex_;
+
+    // [service attributes]
+
+    // [client attributes]
+
+    // [action server attributes]
+    ObstacleDetectionNormalsAlgorithm obstacle_detect;
+    // [action client attributes]
+
+  public:
+   /**
+    * \brief Constructor
+    * 
+    * This constructor initializes specific class attributes and all ROS
+    * communications variables to enable message exchange.
+    */
+    ObstacleDetectionNormalsAlgNode(void);
+
+   /**
+    * \brief Destructor
+    * 
+    * This destructor frees all necessary dynamic memory allocated within this
+    * this class.
+    */
+    ~ObstacleDetectionNormalsAlgNode(void);
+
+  protected:
+   /**
+    * \brief main node thread
+    *
+    * This is the main thread node function. Code written here will be executed
+    * in every node loop while the algorithm is on running state. Loop frequency 
+    * can be tuned by modifying loop_rate attribute.
+    *
+    * Here data related to the process loop or to ROS topics (mainly data structs
+    * related to the MSG and SRV files) must be updated. ROS publisher objects 
+    * must publish their data in this process. ROS client servers may also
+    * request data to the corresponding server topics.
+    */
+    void mainNodeThread(void);
+
+   /**
+    * \brief dynamic reconfigure server callback
+    * 
+    * This method is called whenever a new configuration is received through
+    * the dynamic reconfigure. The derivated generic algorithm class must 
+    * implement it.
+    *
+    * \param config an object with new configuration from all algorithm 
+    *               parameters defined in the config file.
+    * \param level  integer referring the level in which the configuration
+    *               has been changed.
+    */
+    void node_config_update(Config &config, uint32_t level);
+
+   /**
+    * \brief node add diagnostics
+    *
+    * In this abstract function additional ROS diagnostics applied to the 
+    * specific algorithms may be added.
+    */
+    void addNodeDiagnostics(void);
+
+    // [diagnostic functions]
+    
+    // [test functions]
+    
+};
+
+#endif

CMakeLists.txt

+# ******************************************************************** 
+#                 The new CMakeLists.txt file starts here                
+# ******************************************************************** 
+cmake_minimum_required(VERSION 2.8.3)
+project(iri_obstacle_detection_normals)
+
+## Find catkin macros and libraries
+# find_package(catkin REQUIRED)
+# ******************************************************************** 
+#                 Add catkin additional components here
+# ******************************************************************** 
+find_package(catkin REQUIRED COMPONENTS iri_base_algorithm sensor_msgs pcl_ros)
+
+## System dependencies are found with CMake's conventions
+# find_package(Boost REQUIRED COMPONENTS system)
+
+# ******************************************************************** 
+#           Add system and labrobotica dependencies here
+# ******************************************************************** 
+# find_package(<dependency> REQUIRED)
+
+# ******************************************************************** 
+#           Add topic, service and action definition here
+# ******************************************************************** 
+## Generate messages in the 'msg' folder
+# add_message_files(
+#   FILES
+#   Message1.msg
+#   Message2.msg
+# )
+
+## Generate services in the 'srv' folder
+# add_service_files(
+#   FILES
+#   Service1.srv
+#   Service2.srv
+# )
+
+## Generate actions in the 'action' folder
+# add_action_files(
+#   FILES
+#   Action1.action
+#   Action2.action
+# )
+
+## Generate added messages and services with any dependencies listed here
+# generate_messages(
+#   DEPENDENCIES
+#   std_msgs  # Or other packages containing msgs
+# )
+
+# ******************************************************************** 
+#                 Add the dynamic reconfigure file 
+# ******************************************************************** 
+generate_dynamic_reconfigure_options(cfg/ObstacleDetectionNormals.cfg)
+
+# ******************************************************************** 
+#                 Add run time dependencies here
+# ******************************************************************** 
+catkin_package(
+#  INCLUDE_DIRS 
+#  LIBRARIES 
+# ******************************************************************** 
+#            Add ROS and IRI ROS run time dependencies
+# ******************************************************************** 
+  CATKIN_DEPENDS iri_base_algorithm sensor_msgs pcl_ros
+# ******************************************************************** 
+#      Add system and labrobotica run time dependencies here
+# ******************************************************************** 
+#  DEPENDS 
+)
+
+###########
+## Build ##
+###########
+
+# ******************************************************************** 
+#                   Add the include directories 
+# ******************************************************************** 
+include_directories(include)
+include_directories(${catkin_INCLUDE_DIRS})
+include_directories(${iriutils_INCLUDE_DIR} ./include)
+
+## Declare a cpp library
+# add_library(${PROJECT_NAME} <list of source files>)
+
+## Declare a cpp executable
+add_executable(${PROJECT_NAME} src/obstacle_detection_normals_alg.cpp src/obstacle_detection_normals_alg_node.cpp)
+
+# ******************************************************************** 
+#                   Add the libraries
+# ******************************************************************** 
+target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES})
+target_link_libraries(${PROJECT_NAME} ${iriutils_LIBRARY})
+
+# ******************************************************************** 
+#               Add message headers dependencies 
+# ******************************************************************** 
+# add_dependencies(${PROJECT_NAME} <msg_package_name>_generate_messages_cpp)
+# ******************************************************************** 
+#               Add dynamic reconfigure dependencies 
+# ******************************************************************** 
+add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS})

Makefile

+include $(shell rospack find mk)/cmake.mk
\ No newline at end of file

cfg/ObstacleDetectionNormals.cfg

+#! /usr/bin/env python
+#*  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.
+#***********************************************************
+
+# Author: 
+
+PACKAGE='iri_obstacle_detection_normals'
+
+from dynamic_reconfigure.parameter_generator_catkin import *
+
+gen = ParameterGenerator()
+
+#       Name                       Type       Reconfiguration level            Description                       Default   Min   Max
+#gen.add("velocity_scale_factor",  double_t,  0,                               "Maximum velocity scale factor",  0.5,      0.0,  1.0)
+gen.add(       "filter_min_dist",  double_t,  0,                                      "Min distance threshold",  0.3,        0,  100)
+gen.add(        "normal_KSearch",     int_t,  0,                     "No of neighbors to estimate the surface",  100,       10,  200)
+gen.add(              "normal_x",  double_t,  0, 														  	 "Accepted Normal X component",    1,       )
+gen.add(              "normal_y",  double_t,  0, 															   "Accepted Normal Y component", -0.2,       )
+gen.add(              "normal_z",  double_t,  0, 															   "Accepted Normal Z component",    1,       )
+exit(gen.generate(PACKAGE, "ObstacleDetectionNormalsAlgorithm", "ObstacleDetectionNormals"))

docs/ObstacleDetectionNormalsConfig-usage.dox

+\subsubsection usage Usage
+\verbatim
+<node name="ObstacleDetectionNormalsAlgorithm" pkg="iri_obstacle_detection_normals" type="ObstacleDetectionNormalsAlgorithm">
+  <param name="filter_min_dist" type="double" value="0.3" />
+  <param name="normal_KSearch" type="int" value="100" />
+  <param name="normal_x" type="double" value="1.0" />
+  <param name="normal_y" type="double" value="-0.2" />
+  <param name="normal_z" type="double" value="1.0" />
+</node>
+\endverbatim
+