Skip to content
Snippets Groups Projects
Commit 5165d31c authored by Sergi Hernandez's avatar Sergi Hernandez
Browse files

Re-organized the code. 

Added configuration and launch files.
parent 0e75a8c6
No related branches found
No related tags found
No related merge requests found
Hide whitespace changes
Inline Side-by-side
Showing
with 82 additions and 153 deletions
cfg/ObstacleDetectionNormals.cfg
+ 7
− 7
View file @ 5165d31c
......@@ -37,11 +37,11 @@ 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, )
# 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"))
config/default_config.yaml 0 → 100755
+ 5
− 0
View file @ 5165d31c
filter_min_dist: 0.3
normal_KSearchi: 30
normal_x: 1.001
normal_y: 1.001
normal_z: 0.95
include/obstacle_detection_normals_alg.h
+ 4
− 18
View file @ 5165d31c
......@@ -28,7 +28,6 @@
#include <iri_nav_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>
......@@ -38,10 +37,7 @@
#include <boost/make_shared.hpp>
using namespace pcl;
using namespace std;
typedef KdTree<PointXYZ>::Ptr KdTreePtr;
typedef pcl::KdTree<pcl::PointXYZ>::Ptr KdTreePtr;
//include obstacle_detection_normals_alg main library
......@@ -61,17 +57,8 @@ class ObstacleDetectionNormalsAlgorithm
*/
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
......@@ -151,10 +138,9 @@ class ObstacleDetectionNormalsAlgorithm
// 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);
void cloud_all(const pcl::PointCloud<pcl::PointXYZ>& cloud,
pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr& cloud_out,
pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr& cloud_obs);
};
......
include/obstacle_detection_normals_alg_node.h
+ 0
− 6
View file @ 5165d31c
......@@ -54,10 +54,6 @@ using namespace std;
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_;
......@@ -65,8 +61,6 @@ class ObstacleDetectionNormalsAlgNode : public algorithm_base::IriBaseAlgorithm<
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);
......
launch/node.launch 0 → 100644
+ 24
− 0
View file @ 5165d31c
<?xml version="1.0" encoding="UTF-8"?>
<launch>
<arg name="ns" default="ana"/>
<arg name="node_name" default="obstacle_detection_normals"/>
<arg name="config_file" default="$(find iri_nav_obstacle_detection_normals)/config/default_config.yaml"/>
<arg name="output" default="screen"/>
<arg name="launch_prefix" default=""/>
<arg name="cloud_in" default="~pointcloud_in"/>
<group ns="$(arg ns)">
<node pkg ="iri_nav_obstacle_detection_normals"
type="iri_nav_obstacle_detection_normals"
name="$(arg node_name)"
output="$(arg output)"
launch-prefix="$(arg launch_prefix)">
<remap from="~input" to="$(arg cloud_in)"/>
<rosparam file="$(arg config_file)" command="load"/>
</node>
</group>
</launch>
launch/nodelet.launch 0 → 100644
+ 25
− 0
View file @ 5165d31c
<?xml version="1.0" encoding="UTF-8"?>
<launch>
<arg name="ns" default="ana"/>
<arg name="node_name" default="average_point_cloud"/>
<arg name="camera_nodelet_manager" default="camera_nodelet_manager"/>
<arg name="config_file" default="$(find iri_nav_average_point_cloud)/config/default_config.yaml"/>
<arg name="output" default="screen"/>
<arg name="launch_prefix" default=""/>
<arg name="cloud_in" default="~pointcloud_in"/>
<group ns="$(arg ns)">
<node name="$(arg node_name)"
pkg="nodelet"
type="nodelet"
args="load iri_nav_average_point_cloud/AveragePointcloudAlgNodelet $(arg camera_nodelet_manager)"
output="$(arg output)">
<remap from="~input" to="$(arg cloud_in)"/>
<rosparam file="$(arg config_file)" command="load"/>
</node>
</group>
</launch>
src/obstacle_detection_normals_alg.cpp
+ 13
− 79
View file @ 5165d31c
......@@ -26,85 +26,36 @@ the KSearch nearest neighbors. After that, it checks that each normal is inside
some thresholds to determine if it is a passable point or an obstacle. If some point
is nearer than a minimum distance, it is marked in a diferent color.
*/
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)
void ObstacleDetectionNormalsAlgorithm::cloud_all(const pcl::PointCloud<pcl::PointXYZ>& cloud,
pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr& cloud_out,
pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr& cloud_obs)
{
float e=0.0;
//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; }
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
n.setKSearch(this->config_.normal_KSearch);
//n.setViewPoint (0.26374, 0.0, -0.0485);
//estimate
n.setInputCloud(cloud.makeShared());
n.compute(*output);
pcl::concatenateFields (cloud, *output, *cloud_out);
float e=0.0;
pcl::concatenateFields(cloud,*output,*cloud_out);
*cloud_obs=*cloud_out;
......@@ -112,21 +63,12 @@ void ObstacleDetectionNormalsAlgorithm::cloud_all(int KSearch, float normal_z,
{
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].x > min_dist)
if (cloud_out->points[pointIndex].x > this->config_.filter_min_dist)
{
//ROS_INFO_STREAM("z_normal: " << cloud_out->points[pointIndex].normal_z);
if (abs(cloud_out->points[pointIndex].normal_z) > normal_z &&
cloud_out->points[pointIndex].normal_y < normal_y &&
cloud_out->points[pointIndex].normal_x < normal_x )
if (abs(cloud_out->points[pointIndex].normal_z) > this->config_.normal_z &&
cloud_out->points[pointIndex].normal_y < this->config_.normal_y &&
cloud_out->points[pointIndex].normal_x < this->config_.normal_x )
{
/*if (pointIndex % 200 == 0)
{
ROS_INFO_STREAM("x_normal: " << cloud_out->points[pointIndex].normal_x);
ROS_INFO_STREAM("y_normal: " << cloud_out->points[pointIndex].normal_y);
ROS_INFO_STREAM("z_normal: " << cloud_out->points[pointIndex].normal_z);
}*/
cloud_out->points[pointIndex].r=0;
cloud_out->points[pointIndex].g=255;
cloud_out->points[pointIndex].b=0;
......@@ -136,13 +78,6 @@ void ObstacleDetectionNormalsAlgorithm::cloud_all(int KSearch, float normal_z,
}
else
{
//ROS_ERROR_STREAM("z_normal: " << cloud_out->points[pointIndex].normal_z);
/*if (pointIndex % 200 == 0)
{
ROS_ERROR_STREAM("x_normal: " << cloud_out->points[pointIndex].normal_x);
ROS_ERROR_STREAM("y_normal: " << cloud_out->points[pointIndex].normal_y);
ROS_ERROR_STREAM("z_normal: " << cloud_out->points[pointIndex].normal_z);
}*/
cloud_out->points[pointIndex].r=255;
cloud_out->points[pointIndex].g=0;
cloud_out->points[pointIndex].b=0;
......@@ -159,5 +94,4 @@ void ObstacleDetectionNormalsAlgorithm::cloud_all(int KSearch, float normal_z,
}
}
e=(e/float(cloud_out->width*cloud_out->height))*100;
//printf("error %f",e);
}
src/obstacle_detection_normals_alg_node.cpp
+ 4
− 43
View file @ 5165d31c
#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)
algorithm_base::IriBaseAlgorithm<ObstacleDetectionNormalsAlgorithm>()
{
//init class attributes if necessary
//this->loop_rate_ = 2;//in [Hz]
......@@ -49,41 +48,21 @@ void ObstacleDetectionNormalsAlgNode::input_callback(const sensor_msgs::PointClo
// 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);
obstacle_detect.cloud_all(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_);
pcl::toROSMsg (*cloud_out, all_rg_msg_);
pcl::toROSMsg (*cloud_obs, obstacles_msg_);
sensor_msgs::convertPointCloud2ToPointCloud(obstacles_msg_, obstaclespc_msg_);
......@@ -91,17 +70,6 @@ void ObstacleDetectionNormalsAlgNode::input_callback(const sensor_msgs::PointClo
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] */
......@@ -112,13 +80,6 @@ void ObstacleDetectionNormalsAlgNode::input_callback(const sensor_msgs::PointClo
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)
......
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment