Merge branch 'development' into 'master'

Used a PointCloud2 instead of a scan.

See merge request !2

cfg/LidarObstacleDetector.cfg

@@ -50,6 +50,7 @@ gen.add("max_inc",                 double_t,  0,                               "
 gen.add("min_inc",                 double_t,  0,                               "minimum inclination to take into account",0.05,0,0.5)
 gen.add("inc_area_filter",         int_t,     0,                               "Minimum inclination area to take into account",10,1,1000)
 gen.add("max_ring",                int_t,     0,                               "Maximum ring to process"      ,  10,       0,    15)
+gen.add("angle_resolution",        double_t,  0,                               "Angle resolution for the free space pointcloud",0.001,0.0001,0.1)
 # farthest point computation
 gen.add("farthest_enable",         bool_t,    0,                               "Enable the computation of the farthest point",True)
 gen.add("farthest_num_rings",      int_t,     0,                               "Number of rings to use in the farthest point computation",9,1,16)

config/default_config.yaml

 # sensor information
-#base_frame_id: "ana/base_footprint"
+base_frame_id: "base_footprint"
 min_range: 0.1
 max_range: 50.0
 sensor_height: 0.3

include/lidar_obstacle_detector_alg.h

@@ -131,6 +131,7 @@ class LidarObstacleDetectorAlgorithm
 
     // here define all lidar_obstacle_detector_alg interface methods to retrieve and set
     // the driver parameters
+    std::vector<std::string> get_strings(const ros::NodeHandle& nh, const std::string& param_name);
 
    /**
     * \brief Destructor

include/lidar_obstacle_detector_alg_node.h

@@ -55,6 +55,11 @@ typedef struct
   bool init;
 }TLidarData;
 
+typedef struct{
+  double max;
+  double min;
+}TFilterRange;
+
 /**
  * \brief IRI ROS Specific Algorithm Class
  *
@@ -68,8 +73,10 @@ class LidarObstacleDetectorAlgNode : public algorithm_base::IriBaseAlgorithm<Lid
     ros::Publisher obstacles_img_publisher_;
     sensor_msgs::Image obstacles_img_Image_msg_;
 
-    ros::Publisher scan_out_publisher_;
-    sensor_msgs::LaserScan scan_out_LaserScan_msg_;
+    ros::Publisher obstacles_publisher_;
+    sensor_msgs::PointCloud2 obstacles_PointCloud2_msg_;
+    ros::Publisher free_space_publisher_;
+    sensor_msgs::PointCloud2 free_space_PointCloud2_msg_;
 
     // [subscriber attributes]
     ros::Subscriber pointcloud_in_subscriber_;
@@ -82,6 +89,7 @@ class LidarObstacleDetectorAlgNode : public algorithm_base::IriBaseAlgorithm<Lid
     pcl::PointCloud<pcl::PointXYZ>::Ptr transformed_input;
     bool new_point_cloud;
 
+
     // [service attributes]
 
     // [client attributes]
@@ -92,9 +100,11 @@ class LidarObstacleDetectorAlgNode : public algorithm_base::IriBaseAlgorithm<Lid
 
     tf::TransformListener listener;
 
+    unsigned int max_col_;
     cv::Mat inc_image;
     cv::Mat label_image;
     std::vector<TLidarData> lidar_data;
+    std::vector<TFilterRange> ranges;
 
    /**
     * \brief config variable
@@ -157,8 +167,13 @@ class LidarObstacleDetectorAlgNode : public algorithm_base::IriBaseAlgorithm<Lid
     */
     void addNodeDiagnostics(void);
 
+    pcl::PointCloud<pcl::PointXYZ>::Ptr transform_pointcloud(pcl::PointCloud<pcl::PointXYZ>::Ptr &input);
     bool get_point(unsigned int ring, unsigned int col,pcl::PointXYZ &point, double &range);
     double get_max_inclination(double current_angle,pcl::PointXYZ &point1,double range1,pcl::PointXYZ &point2,double range2);
+    void init_free_space(std::vector<bool> &free_space);
+    void compute_inclination(void);
+    void virtual_obstacles(std::vector<pcl::PointXYZ> &points,std::vector<bool> &free_space);
+    void farthest_obstacles(std::vector<pcl::PointXYZ> &points,std::vector<bool> &free_space);
 
     // [diagnostic functions]
     

launch/node.launch

@@ -4,10 +4,10 @@
   <arg name="ns" default="ana"/>
   <arg name="node_name" default="lidar_obstacle_detector"/>
   <arg name="config_file" default="$(find iri_lidar_obstacle_detector)/config/default_config.yaml"/>
+  <arg name="ranges_file" default="$(find iri_lidar_obstacle_detector)/config/default_ranges.yaml"/>
   <arg name="output" default="screen"/>
   <arg name="launch_prefix" default=""/>
   <arg name="cloud_in" default="~pointcloud_in"/>
-  <arg name="base_frame_id" default="$(arg ns)/base_footprint"/>
 
   <group ns="$(arg ns)">
 
@@ -18,7 +18,7 @@
           launch-prefix="$(arg launch_prefix)">
       <remap from="~pointcloud_in" to="$(arg cloud_in)"/>
       <rosparam file="$(arg config_file)" command="load"/>
-      <param name="base_frame_id" value="$(arg base_frame_id)"/>
+      <rosparam file="$(arg ranges_file)" command="load"/>
     </node>
 
   </group>

launch/nodelet.launch

@@ -5,10 +5,10 @@
   <arg name="node_name" default="lidar_obstacle_detector"/>
   <arg name="lidar_nodelet_manager" default="lidar_nodelet_manager"/>
   <arg name="config_file" default="$(find iri_lidar_obstacle_detector)/config/default_config.yaml"/>
+  <arg name="ranges_file" default="$(find iri_lidar_obstacle_detector)/config/default_ranges.yaml"/>
   <arg name="output" default="screen"/>
   <arg name="launch_prefix" default=""/>
   <arg name="cloud_in" default="~pointcloud_in"/>
-  <arg name="base_frame_id" default="$(arg ns)/base_footprint"/>
 
   <group ns="$(arg ns)">
 
@@ -19,7 +19,7 @@
           output="$(arg output)">
       <remap from="~pointcloud_in" to="$(arg cloud_in)"/>
       <rosparam file="$(arg config_file)" command="load"/>
-      <param name="base_frame_id" value="$(arg base_frame_id)"/>
+      <rosparam file="$(arg ranges_file)" command="load"/>
     </node>
 
   </group>

src/lidar_obstacle_detector_alg.cpp

@@ -10,6 +10,33 @@ LidarObstacleDetectorAlgorithm::~LidarObstacleDetectorAlgorithm(void)
   pthread_mutex_destroy(&this->access_);
 }
 
+std::vector<std::string> LidarObstacleDetectorAlgorithm::get_strings(const ros::NodeHandle& nh, const std::string& param_name)
+{
+  XmlRpc::XmlRpcValue xml_array;
+  if(!nh.getParam(param_name,xml_array))
+  {
+    ROS_ERROR_STREAM("PointCloudAngleFilterAlg::get_strings: Could not find '" << param_name << "' parameter (namespace: " << nh.getNamespace() << ").");
+    return std::vector<std::string>();
+  }
+  if(xml_array.getType()!=XmlRpc::XmlRpcValue::TypeArray)
+  {
+    ROS_ERROR_STREAM("PointCloudAngleFilterAlg::get_strings: The '" << param_name << "' parameter is not an array (namespace: " << nh.getNamespace() << ").");
+    return std::vector<std::string>();
+  }
+
+  std::vector<std::string> out;
+  for(int i=0;i<xml_array.size();++i)
+  {
+    if(xml_array[i].getType()!=XmlRpc::XmlRpcValue::TypeString)
+    {
+      ROS_ERROR_STREAM("PointCloudAngleFilterAlg::get_strings: The '" << param_name << "' parameter contains a non-string element (namespace: " << nh.getNamespace() << ").");
+      return std::vector<std::string>();
+    }
+    out.push_back(static_cast<std::string>(xml_array[i]));
+  }
+  return out;
+}
+
 void LidarObstacleDetectorAlgorithm::config_update(Config& config, uint32_t level)
 {
   this->lock();