diff --git a/.svn/entries b/.svn/entries deleted file mode 100644 index 48082f72f087ce7e6fa75b9c41d7387daecd447b..0000000000000000000000000000000000000000 --- a/.svn/entries +++ /dev/null @@ -1 +0,0 @@ -12 diff --git a/.svn/format b/.svn/format deleted file mode 100644 index 48082f72f087ce7e6fa75b9c41d7387daecd447b..0000000000000000000000000000000000000000 --- a/.svn/format +++ /dev/null @@ -1 +0,0 @@ -12 diff --git a/.svn/pristine/03/034503d8de6d1e291d61d80180160aaa930b1078.svn-base b/.svn/pristine/03/034503d8de6d1e291d61d80180160aaa930b1078.svn-base deleted file mode 100644 index da40b5f2261b9773fe467f4d93743f54416a85dd..0000000000000000000000000000000000000000 --- a/.svn/pristine/03/034503d8de6d1e291d61d80180160aaa930b1078.svn-base +++ /dev/null @@ -1,52 +0,0 @@ -#! /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_hole_detection' -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( "hole_min_p", int_t, 0, "No of Points to be hole (less points is a hole)", 30, 5, 200) -gen.add( "box_x", double_t, 0, "X distance hole detection zone", 0.6, 0, 1.5) -gen.add( "box_y_ini", double_t, 0, "Y distance hole detection zone", 0, -0.5, 0.5) -gen.add( "box_y_end", double_t, 0, "Initial Y hole detection zone", 0.1, -0.5, 0.5) -gen.add( "box_z_ini", double_t, 0, "Initial Z hole detection zone", 0.8, 0, 2) -gen.add( "box_z_end", double_t, 0, "Z distance hole detection zone", 0.9, 0.5, 2) - - - -exit(gen.generate(PACKAGE, "HoleDetectionAlgorithm", "HoleDetection")) diff --git a/.svn/pristine/19/1978df7856af4709da5e821ec14e43f9884a0dca.svn-base b/.svn/pristine/19/1978df7856af4709da5e821ec14e43f9884a0dca.svn-base deleted file mode 100644 index 7baf19e462e4b6fce59bfe7969e1db38556dc1d9..0000000000000000000000000000000000000000 --- a/.svn/pristine/19/1978df7856af4709da5e821ec14e43f9884a0dca.svn-base +++ /dev/null @@ -1,11 +0,0 @@ -\subsubsection parameters ROS parameters - -Reads and maintains the following parameters on the ROS server - -- \b "~hole_min_p" : \b [int] No of Points to be hole (less points is a hole) min: 5, default: 30, max: 200 -- \b "~box_x" : \b [double] X distance hole detection zone min: 0.0, default: 0.6, max: 1.5 -- \b "~box_y_ini" : \b [double] Y distance hole detection zone min: -0.5, default: 0.0, max: 0.5 -- \b "~box_y_end" : \b [double] Initial Y hole detection zone min: -0.5, default: 0.1, max: 0.5 -- \b "~box_z_ini" : \b [double] Initial Z hole detection zone min: 0.0, default: 0.8, max: 2.0 -- \b "~box_z_end" : \b [double] Z distance hole detection zone min: 0.5, default: 0.9, max: 2.0 - diff --git a/.svn/pristine/4c/4c66109c45be5a80a3f9f6f26f0b7c1584c2cf15.svn-base b/.svn/pristine/4c/4c66109c45be5a80a3f9f6f26f0b7c1584c2cf15.svn-base deleted file mode 100644 index 23bc74096de7d471c370fe1a8456365e3cdcea3a..0000000000000000000000000000000000000000 --- a/.svn/pristine/4c/4c66109c45be5a80a3f9f6f26f0b7c1584c2cf15.svn-base +++ /dev/null @@ -1,136 +0,0 @@ -#include "hole_detection_alg_node.h" - -HoleDetectionAlgNode::HoleDetectionAlgNode(void) : - algorithm_base::IriBaseAlgorithm<HoleDetectionAlgorithm>(),hole_min_p(60),box_y_end(0),box_z_ini(1), - box_y_ini(-0.1),box_z_end(1.8),box_x(0.9) -{ - //init class attributes if necessary - //this->loop_rate_ = 2;//in [Hz] - - // [init publishers] - this->hole_obstacle_publisher_ = this->public_node_handle_.advertise<sensor_msgs::PointCloud>("hole_obstacle", 10); - this->hole_all_publisher_ = this->public_node_handle_.advertise<sensor_msgs::PointCloud2>("hole_zone", 10); - // [init subscribers] - this->input_subscriber_ = this->public_node_handle_.subscribe("input", 10, &HoleDetectionAlgNode::input_callback, this); - - - // [init services] - - // [init clients] - - // [init action servers] - - // [init action clients] -} - -HoleDetectionAlgNode::~HoleDetectionAlgNode(void) -{ - // [free dynamic memory] -} - -void HoleDetectionAlgNode::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] -} -//PointCloud2_msg_ -/* [subscriber callbacks] */ -void HoleDetectionAlgNode::input_callback(const sensor_msgs::PointCloud2::ConstPtr& msg) -{ - ROS_INFO("HoleDetectionAlgNode::input_callback: New Message Received"); - - - - pcl::fromROSMsg (*msg, cloud_in); - -///////////////////////////////////////////////////////////////////////////////////// - pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_for (new pcl::PointCloud<pcl::PointXYZ>); - cloud_for->header.frame_id = cloud_in.header.frame_id; - cloud_for->is_dense=false; - cloud_for->header.stamp = ros::Time::now (); -///////////////////////////////////////////////////////////////////////////////////// - - //use appropiate mutex to shared variables if necessary - - this->alg_.lock(); - - cloud_in_rgb.points.resize(cloud_in.size()); - for (size_t i = 0; i < cloud_in.points.size(); i++) { - cloud_in_rgb.points[i].x = cloud_in.points[i].x; - cloud_in_rgb.points[i].y = cloud_in.points[i].y; - cloud_in_rgb.points[i].z = cloud_in.points[i].z; - cloud_in_rgb.points[i].r = 0; - cloud_in_rgb.points[i].g = 0; - cloud_in_rgb.points[i].b = 0; - } - cloud_in_rgb.header.frame_id = cloud_in.header.frame_id; - cloud_in_rgb.height = cloud_in.height; - cloud_in_rgb.width = cloud_in.width; - // cloud_in_rgb.fields= - // cloud_in_rgb.is_bigendian= - // cloud_in_rgb.point_step= - // cloud_in_rgb.row_step= - // cloud_in_rgb.is_dense= - - cloud_in_rgb.is_dense=false; - cloud_in_rgb.header.stamp = ros::Time::now (); - - hole_detect.cloud_all(hole_min_p, box_y_end, box_z_ini, box_x, box_y_ini, box_z_end, Xl, Xc, Xr, Y, cloud_in_rgb, cloud_for); - //unlock previously blocked shared variables - this->alg_.unlock(); - -/////////////////////////////////////////////////////////////////////////////////////// - - pcl::toROSMsg (*cloud_for, PointCloud2_msg_); - pcl::toROSMsg (cloud_in_rgb, PointCloud2_msg_all); - - sensor_msgs::convertPointCloud2ToPointCloud(PointCloud2_msg_, PointCloud_msg_); - - - this->hole_obstacle_publisher_.publish(this->PointCloud_msg_); - this->hole_all_publisher_.publish(this->PointCloud2_msg_all); - - //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 HoleDetectionAlgNode::node_config_update(Config &config, uint32_t level) -{ - this->alg_.lock(); -hole_min_p=config.hole_min_p; -box_y_end=config.box_y_end; -box_z_ini=config.box_z_ini; -box_z_end=config.box_z_end; -box_y_ini=config.box_y_ini; -box_x=config.box_x; - this->alg_.unlock(); -} - -void HoleDetectionAlgNode::addNodeDiagnostics(void) -{ -} - -/* main function */ -int main(int argc,char *argv[]) -{ - return algorithm_base::main<HoleDetectionAlgNode>(argc, argv, "hole_detection_alg_node"); -} diff --git a/.svn/pristine/4f/4fde0d083eff790481d78aef523f9ecb46456cf1.svn-base b/.svn/pristine/4f/4fde0d083eff790481d78aef523f9ecb46456cf1.svn-base deleted file mode 100644 index 3c60b91cdb449def68082574bf3dfb987a9975da..0000000000000000000000000000000000000000 --- a/.svn/pristine/4f/4fde0d083eff790481d78aef523f9ecb46456cf1.svn-base +++ /dev/null @@ -1,32 +0,0 @@ -# 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= ~hole_min_p -0.default= 30 -0.type= int -0.desc=No of Points to be hole (less points is a hole) Range: 5 to 200 -1.name= ~box_x -1.default= 0.6 -1.type= double -1.desc=X distance hole detection zone Range: 0.0 to 1.5 -2.name= ~box_y_ini -2.default= 0.0 -2.type= double -2.desc=Y distance hole detection zone Range: -0.5 to 0.5 -3.name= ~box_y_end -3.default= 0.1 -3.type= double -3.desc=Initial Y hole detection zone Range: -0.5 to 0.5 -4.name= ~box_z_ini -4.default= 0.8 -4.type= double -4.desc=Initial Z hole detection zone Range: 0.0 to 2.0 -5.name= ~box_z_end -5.default= 0.9 -5.type= double -5.desc=Z distance hole detection zone Range: 0.5 to 2.0 -} -} -# End of autogenerated section. You may edit below. diff --git a/.svn/pristine/5f/5fd21080e7e0887f27d44d491be4fa1c7e814e69.svn-base b/.svn/pristine/5f/5fd21080e7e0887f27d44d491be4fa1c7e814e69.svn-base deleted file mode 100644 index cdbf7010b655228b8d1b997ed2d1f9aca55fb968..0000000000000000000000000000000000000000 --- a/.svn/pristine/5f/5fd21080e7e0887f27d44d491be4fa1c7e814e69.svn-base +++ /dev/null @@ -1,43 +0,0 @@ -cmake_minimum_required(VERSION 2.4.6) -include($ENV{ROS_ROOT}/core/rosbuild/rosbuild.cmake) - -set(PROJECT_NAME hole_detection_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) - -INCLUDE_DIRECTORIES(${iriutils_INCLUDE_DIR} ./include) - -#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/hole_detection_alg.cpp src/hole_detection_alg_node.cpp) - -target_link_libraries(${PROJECT_NAME} ${iriutils_LIBRARY}) diff --git a/.svn/pristine/6b/6b1a5374f47019762a151de51830bad60889affb.svn-base b/.svn/pristine/6b/6b1a5374f47019762a151de51830bad60889affb.svn-base deleted file mode 100644 index bdce98c51e4745112e8354e8a0f426f3882420dc..0000000000000000000000000000000000000000 --- a/.svn/pristine/6b/6b1a5374f47019762a151de51830bad60889affb.svn-base +++ /dev/null @@ -1,105 +0,0 @@ -#include "hole_detection_alg.h" -using namespace std; - -HoleDetectionAlgorithm::HoleDetectionAlgorithm(void) -{ -} - -HoleDetectionAlgorithm::~HoleDetectionAlgorithm(void) -{ -} - -void HoleDetectionAlgorithm::config_update(Config& new_cfg, uint32_t level) -{ - this->lock(); - - // save the current configuration - this->config_=new_cfg; - - this->unlock(); -} - -// HoleDetectionAlgorithm Public API - -void HoleDetectionAlgorithm::cloud_all(int hole_min_p, float box_y_end, float box_z_ini, - float box_x, float box_y_ini, float box_z_end, - float Xl, float Xc, float Xr, float Y, - pcl::PointCloud<pcl::PointXYZRGB>& cloud_in, - pcl::PointCloud<pcl::PointXYZ>::Ptr& cloud_for) -{ -int l=0; -int c=0; -int r=0; - - -for (size_t rowIndex=0, pointIndex=0; rowIndex<cloud_in.height; ++rowIndex) -{ - for (size_t colIndex=0; colIndex<cloud_in.width; ++colIndex, ++pointIndex) - { - if (box_y_ini < cloud_in.points[pointIndex].y && - cloud_in.points[pointIndex].y < box_y_end && - box_z_ini < cloud_in.points[pointIndex].z && - cloud_in.points[pointIndex].z < box_z_end) - { - if (-(box_x/2) < cloud_in.points[pointIndex].x && - cloud_in.points[pointIndex].x < ((box_x/3)-(box_x/2))) - { - cloud_in.points[pointIndex].r=0; - cloud_in.points[pointIndex].g=0; - cloud_in.points[pointIndex].b=255; - l=l+1; - } - if (((box_x/3)-(box_x/2)) < cloud_in.points[pointIndex].x && - cloud_in.points[pointIndex].x < ((box_x/2)-(box_x/3))) - { - cloud_in.points[pointIndex].r=0; - cloud_in.points[pointIndex].g=255; - cloud_in.points[pointIndex].b=255; - c=c+1; - } - if (((box_x/2)-(box_x/3)) < cloud_in.points[pointIndex].x && - cloud_in.points[pointIndex].x < (box_x/2)) - { - cloud_in.points[pointIndex].r=0; - cloud_in.points[pointIndex].g=127; - cloud_in.points[pointIndex].b=255; - r=r+1; - } - } - } -} - -////////////////////////////////////////////////////////////////////////////// -// Hole Point Cloud -////////////////////////////////////////////////////////////////////////////// - - -Y=-0.1; - -for (size_t rowIndex=0, pointIndex=0; rowIndex<5; ++rowIndex) -{ - Xl= -(box_x/2); - Xc= ((box_x/3)-(box_x/2)); - Xr= ((box_x/2)-(box_x/3)); - for (size_t colIndex=0; colIndex<5; ++colIndex, ++pointIndex) - { - if (l<hole_min_p) - { - cloud_for->points.push_back (pcl::PointXYZ(Xl+0.05, Y, box_z_ini)); - Xl=Xl+0.05; - } - if (c<hole_min_p) - { - cloud_for->points.push_back (pcl::PointXYZ(Xc+0.05, Y, box_z_ini)); - Xc=Xc+0.05; - } - if (r<hole_min_p) - { - cloud_for->points.push_back (pcl::PointXYZ(Xr+0.05, Y, box_z_ini)); - Xr=Xr+0.05; - } - } - Y=Y-0.05; -} - -} \ No newline at end of file diff --git a/.svn/pristine/83/835c28c1892b24fa882e60d2a386c7026074bb1e.svn-base b/.svn/pristine/83/835c28c1892b24fa882e60d2a386c7026074bb1e.svn-base deleted file mode 100644 index a9d24a01741a4faf3e32e53faad371581d48078f..0000000000000000000000000000000000000000 --- a/.svn/pristine/83/835c28c1892b24fa882e60d2a386c7026074bb1e.svn-base +++ /dev/null @@ -1,144 +0,0 @@ -// 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 _hole_detection_alg_h_ -#define _hole_detection_alg_h_ - -#include <iri_hole_detection/HoleDetectionConfig.h> -#include "mutex.h" - -#include <pcl_ros/point_cloud.h> -#include <pcl/ros/conversions.h> -#include <pcl/point_types.h> -#include <pcl/impl/point_types.hpp> - - -using namespace pcl; -using namespace std; -//include hole_detection_alg main library - -/** - * \brief IRI ROS Specific Driver Class - * - * - */ -class HoleDetectionAlgorithm -{ - protected: - /** - * \brief define config type - * - * Define a Config type with the HoleDetectionConfig. All driver implementations - * will then use the same variable type Config. - */ - CMutex alg_mutex_; - - // private attributes and methods - int hole_min_p; - float box_y_end,box_z_ini,box_x,box_y_ini,box_z_end,Xl,Xc,Xr,Y; - pcl::PointCloud<pcl::PointXYZRGB> cloud_in; - pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_for; - public: - /** - * \brief define config type - * - * Define a Config type with the HoleDetectionConfig. All driver implementations - * will then use the same variable type Config. - */ - typedef iri_hole_detection::HoleDetectionConfig 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. - */ - HoleDetectionAlgorithm(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 hole_detection_alg interface methods to retrieve and set - // the driver parameters - - /** - * \brief Destructor - * - * This destructor is called when the object is about to be destroyed. - * - */ - ~HoleDetectionAlgorithm(void); - - void cloud_all(int hole_min_p, float box_y_end, float box_z_ini, - float box_x, float box_y_ini, float box_z_end, - float Xl, float Xc, float Xr, float Y, - pcl::PointCloud<pcl::PointXYZRGB>& cloud_in, - pcl::PointCloud<pcl::PointXYZ>::Ptr& cloud_out); - -}; - -#endif diff --git a/.svn/pristine/93/9330238c41fce7bc267dc81e03df00c1ba0f1f1f.svn-base b/.svn/pristine/93/9330238c41fce7bc267dc81e03df00c1ba0f1f1f.svn-base deleted file mode 100644 index 426411e5f276158cff06cffbaf570193113624db..0000000000000000000000000000000000000000 --- a/.svn/pristine/93/9330238c41fce7bc267dc81e03df00c1ba0f1f1f.svn-base +++ /dev/null @@ -1,12 +0,0 @@ -\subsubsection usage Usage -\verbatim -<node name="HoleDetectionAlgorithm" pkg="iri_hole_detection" type="HoleDetectionAlgorithm"> - <param name="hole_min_p" type="int" value="30" /> - <param name="box_x" type="double" value="0.6" /> - <param name="box_y_ini" type="double" value="0.0" /> - <param name="box_y_end" type="double" value="0.1" /> - <param name="box_z_ini" type="double" value="0.8" /> - <param name="box_z_end" type="double" value="0.9" /> -</node> -\endverbatim - diff --git a/.svn/pristine/a6/a6f3057559b8184ac094cb3debde5ac0e5b63b1f.svn-base b/.svn/pristine/a6/a6f3057559b8184ac094cb3debde5ac0e5b63b1f.svn-base deleted file mode 100644 index a4cd4fcfcbfc4a1cc363136fb829cb81da52ace0..0000000000000000000000000000000000000000 --- a/.svn/pristine/a6/a6f3057559b8184ac094cb3debde5ac0e5b63b1f.svn-base +++ /dev/null @@ -1,17 +0,0 @@ -<package> - <description brief="iri_hole_detection"> - - iri_hole_detection - - </description> - <author>asantamaria</author> - <license>LGPL</license> - <review status="unreviewed" notes=""/> - <url>http://ros.org/wiki/iri_hole_detection</url> - <depend package="roscpp"/> - <depend package="iri_base_algorithm"/> - <depend package="sensor_msgs"/> - <depend package="pcl_ros"/> -</package> - - diff --git a/.svn/pristine/d1/d19fdb4bb580a6e10a1dd499866acd3a86501c42.svn-base b/.svn/pristine/d1/d19fdb4bb580a6e10a1dd499866acd3a86501c42.svn-base deleted file mode 100644 index bdc9912b08a04b46505b040ad0dd53c2d74fad9d..0000000000000000000000000000000000000000 --- a/.svn/pristine/d1/d19fdb4bb580a6e10a1dd499866acd3a86501c42.svn-base +++ /dev/null @@ -1,145 +0,0 @@ -// 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 _hole_detection_alg_node_h_ -#define _hole_detection_alg_node_h_ - -#include <iri_base_algorithm/iri_base_algorithm.h> -#include "hole_detection_alg.h" - -#include <ros/ros.h> -//#include <ros/publisher.h> - -#include <pcl_ros/point_cloud.h> -#include <pcl/ros/conversions.h> -#include <pcl/point_types.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 HoleDetectionAlgNode : public algorithm_base::IriBaseAlgorithm<HoleDetectionAlgorithm> -{ - private: - - int hole_min_p; - float box_y_end,box_z_ini,box_y_ini,box_z_end,box_x,Xl,Xc,Xr,Y; -// bool L,C,R; - - // [publisher attributes] - ros::Publisher hole_obstacle_publisher_; - sensor_msgs::PointCloud2 PointCloud2_msg_; - sensor_msgs::PointCloud PointCloud_msg_; - - - ros::Publisher hole_all_publisher_; - sensor_msgs::PointCloud2 PointCloud2_msg_all; - pcl::PointCloud<pcl::PointXYZ> cloud_in; - pcl::PointCloud<pcl::PointXYZRGB> cloud_in_rgb; - - // [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] - HoleDetectionAlgorithm hole_detect; - // [action client attributes] - - public: - /** - * \brief Constructor - * - * This constructor initializes specific class attributes and all ROS - * communications variables to enable message exchange. - */ - HoleDetectionAlgNode(void); - - /** - * \brief Destructor - * - * This destructor frees all necessary dynamic memory allocated within this - * this class. - */ - ~HoleDetectionAlgNode(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 diff --git a/.svn/pristine/ee/ee6111b4a20dffe9d1f790538215cb5b823a9ff9.svn-base b/.svn/pristine/ee/ee6111b4a20dffe9d1f790538215cb5b823a9ff9.svn-base deleted file mode 100644 index 1884406135d37652c42317d1b8bb6e88bec9e0b4..0000000000000000000000000000000000000000 --- a/.svn/pristine/ee/ee6111b4a20dffe9d1f790538215cb5b823a9ff9.svn-base +++ /dev/null @@ -1,26 +0,0 @@ -/** -\mainpage -\htmlinclude manifest.html - -\b iri_hole_detection 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. ---> - - -*/ diff --git a/.svn/wc.db b/.svn/wc.db deleted file mode 100644 index b44250c25b539205dcf02775f549a78d550f2c72..0000000000000000000000000000000000000000 Binary files a/.svn/wc.db and /dev/null differ diff --git a/.svn/wc.db-journal b/.svn/wc.db-journal deleted file mode 100644 index e69de29bb2d1d6434b8b29ae775ad8c2e48c5391..0000000000000000000000000000000000000000