topics names from yaml

cfg/GroundSegmentation.cfg

@@ -38,6 +38,13 @@ from dynamic_reconfigure.parameter_generator_catkin import *
 gen = ParameterGenerator()
 
 #       Name                       Type       Reconfiguration level            Description                       Default   Min   Max
+gen.add("rate",  double_t,  0,           "Main loop rate (Hz)",  100.0,   0.1, 1000.0)
+gen.add("input_pointcloud_topic",  str_t,  0, "input_pointcloud_topic",  "/xyzi_to_extended_structure_converter/lidar_points_extended_structure")
+gen.add("output_pointcloud_segmented_topic",  str_t,  0, "output_pointcloud_segmented_topic",  "lidar_points_ground_segmented")
+gen.add("output_roadmap_markers_topic",  str_t,  0, "output_roadmap_markers_topic",  "roadmap_markers")
+gen.add("output_roadmap_nodes_topic",  str_t,  0, "output_roadmap_nodes_topic",  "roadmap_nodes")
+gen.add("output_roadmap_edges_topic",  str_t,  0, "output_roadmap_edges_topic",  "roadmap_edges")
+
 gen.add("sensor_height", double_t, 0, "Height of the sensor center", 1.75, 0.0, 5.0); #KITTI = 1.75: BLUE = 1.3 
 
 gen.add("angle_reduction_factor", double_t, 0, "To make the search area dependent to the data sparseness", 2.0, -100.0, 100.0);

include/ground_segmentation_alg_node.h

@@ -43,6 +43,12 @@ class GroundSegmentationAlgNode : public algorithm_base::IriBaseAlgorithm<
     GroundSegmentationAlgorithm>
 {
 private:
+  std::string input_pointcloud_topic_;
+  std::string output_pointcloud_segmented_topic_;
+  std::string output_roadmap_markers_topic_;
+  std::string output_roadmap_nodes_topic_;
+  std::string output_roadmap_edges_topic_;
+
   std_msgs::Header common_header_;
   bool flag_new_velodyne_data_;
   ros::Publisher velodyne_republisher_;

src/ground_segmentation_alg_node.cpp

@@ -4,26 +4,67 @@ GroundSegmentationAlgNode::GroundSegmentationAlgNode(void) :
     algorithm_base::IriBaseAlgorithm<GroundSegmentationAlgorithm>()
 {
   //init class attributes if necessary
-  //this->loop_rate_ = 100; //in [Hz]
-  this->setRate(100.0); // New iri-core implementation
+  if (!this->private_node_handle_.getParam("rate", this->config_.rate))
+  {
+    ROS_WARN("GroundSegmentationAlgNode::GroundSegmentationAlgNode: param 'rate' not found");
+  }
+  else
+    this->setRate(this->config_.rate);
+
+  if (!this->private_node_handle_.getParam("input_pointcloud_topic", this->config_.input_pointcloud_topic))
+  {
+    ROS_WARN("GroundSegmentationAlgNode::GroundSegmentationAlgNode: param 'input_pointcloud_topic' not found");
+  }
+  else
+    input_pointcloud_topic_ = config_.input_pointcloud_topic;
+
+  if (!this->private_node_handle_.getParam("output_pointcloud_segmented_topic",
+                                           this->config_.output_pointcloud_segmented_topic))
+  {
+    ROS_WARN(
+        "GroundSegmentationAlgNode::GroundSegmentationAlgNode: param 'output_pointcloud_segmented_topic' not found");
+  }
+  else
+    output_pointcloud_segmented_topic_ = config_.output_pointcloud_segmented_topic;
+
+  if (!this->private_node_handle_.getParam("output_roadmap_markers_topic", this->config_.output_roadmap_markers_topic))
+  {
+    ROS_WARN("GroundSegmentationAlgNode::GroundSegmentationAlgNode: param 'output_roadmap_markers' not found");
+  }
+  else
+    output_roadmap_markers_topic_ = config_.output_roadmap_markers_topic;
+
+  if (!this->private_node_handle_.getParam("output_roadmap_nodes_topic", this->config_.output_roadmap_nodes_topic))
+  {
+    ROS_WARN("GroundSegmentationAlgNode::GroundSegmentationAlgNode: param 'output_roadmap_nodes_topic' not found");
+  }
+  else
+    output_roadmap_nodes_topic_ = config_.output_roadmap_nodes_topic;
+
+  if (!this->private_node_handle_.getParam("output_roadmap_edges_topic", this->config_.output_roadmap_edges_topic))
+  {
+    ROS_WARN("GroundSegmentationAlgNode::GroundSegmentationAlgNode: param 'output_roadmap_edges_topic' not found");
+  }
+  else
+    output_roadmap_edges_topic_ = config_.output_roadmap_edges_topic;
 
   flag_new_velodyne_data_ = false;
 
   // [init publishers]
-  this->velodyne_republisher_ = this->public_node_handle_.advertise<sensor_msgs::PointCloud2>(
-      "/EARM/velodyne_ground_segmented", 1);
+  this->velodyne_republisher_ = this->private_node_handle_.advertise < sensor_msgs::PointCloud2
+      > (output_pointcloud_segmented_topic_, 1);
 
-  this->local_goals_publisher_ = this->public_node_handle_.advertise<visualization_msgs::MarkerArray>("/EARM/roadmap",
-                                                                                                      1);
+  this->local_goals_publisher_ = this->private_node_handle_.advertise < visualization_msgs::MarkerArray
+      > (output_roadmap_markers_topic_, 1);
 
-  this->ground_references_publisher_ = this->public_node_handle_.advertise<sensor_msgs::PointCloud2>(
-      "/EARM/ground_references", 1);
+  this->ground_references_publisher_ = this->private_node_handle_.advertise < sensor_msgs::PointCloud2
+      > (output_roadmap_nodes_topic_, 1);
 
-  this->edges_publisher_ = this->public_node_handle_.advertise<iri_ground_segmentation::edges_array>(
-      "/EARM/edges_array", 1);
+  this->edges_publisher_ = this->private_node_handle_.advertise < iri_ground_segmentation::edges_array
+      > (output_roadmap_edges_topic_, 1);
 
   // [init subscribers]
-  this->velodyne_subscriber_ = this->public_node_handle_.subscribe("/velodyne_points", 1,
+  this->velodyne_subscriber_ = this->public_node_handle_.subscribe(input_pointcloud_topic_, 1,
                                                                    &GroundSegmentationAlgNode::cb_velodyne, this);
 
   pthread_mutex_init(&this->velodyne_mutex_, NULL);
@@ -36,8 +77,8 @@ GroundSegmentationAlgNode::~GroundSegmentationAlgNode(void)
   pthread_mutex_destroy(&this->velodyne_mutex_);
 }
 
-void GroundSegmentationAlgNode::rosPointCloudtoPCLPointCloud(const sensor_msgs::PointCloud2& ros_point_cloud,
-                                                             pcl::PointCloud<pcl::PointXYZRGBNormal>& pcl_point_cloud)
+void GroundSegmentationAlgNode::rosPointCloudtoPCLPointCloud(const sensor_msgs::PointCloud2 &ros_point_cloud,
+                                                             pcl::PointCloud<pcl::PointXYZRGBNormal> &pcl_point_cloud)
 {
   pcl::PCLPointCloud2 aux;
   pcl_conversions::toPCL(ros_point_cloud, aux);
@@ -45,7 +86,7 @@ void GroundSegmentationAlgNode::rosPointCloudtoPCLPointCloud(const sensor_msgs::
 }
 
 void GroundSegmentationAlgNode::PCLPointCloudToRosPointCloud(
-    const pcl::PointCloud<pcl::PointXYZRGBNormal>& pcl_point_cloud, sensor_msgs::PointCloud2& ros_point_cloud)
+    const pcl::PointCloud<pcl::PointXYZRGBNormal> &pcl_point_cloud, sensor_msgs::PointCloud2 &ros_point_cloud)
 {
   pcl::PCLPointCloud2 aux;
   toPCLPointCloud2(pcl_point_cloud, aux);
@@ -153,7 +194,7 @@ void GroundSegmentationAlgNode::mainNodeThread(void)
 
 /*  [subscriber callbacks] */
 
-void GroundSegmentationAlgNode::cb_velodyne(const sensor_msgs::PointCloud2::ConstPtr& msg)
+void GroundSegmentationAlgNode::cb_velodyne(const sensor_msgs::PointCloud2::ConstPtr &msg)
 {
   //this->velodyne_mutex_enter();
   velodyne_ros_cloud_ = *msg;
@@ -189,7 +230,8 @@ void GroundSegmentationAlgNode::node_config_update(Config &config, uint32_t leve
   this->alg_.lidar_configuration_.sensor_height = config.sensor_height;
   this->alg_.filtering_configuration_.angle_reduction_factor = config.angle_reduction_factor;
   this->alg_.filtering_configuration_.search_limit_in_shadow_area = config.search_limit_in_shadow_area;
-  this->alg_.filtering_configuration_.distance_to_virtual_references_in_shadow_area = config.distance_to_virtual_references_in_shadow_area;
+  this->alg_.filtering_configuration_.distance_to_virtual_references_in_shadow_area =
+      config.distance_to_virtual_references_in_shadow_area;
 
   this->alg_.filtering_configuration_.propagation_z_additive_noise = config.propagation_z_additive_noise_per_meter;
   this->alg_.filtering_configuration_.propagation_additive_noise = config.propagation_additive_noise_deg_per_meter
@@ -213,7 +255,6 @@ void GroundSegmentationAlgNode::node_config_update(Config &config, uint32_t leve
   this->alg_.filtering_configuration_.measure_performance = config.measure_performance;
   this->alg_.filtering_configuration_.show_dense_reconstruction = config.show_dense_reconstruction;
 
-
   std::cout << "GroundSegmentationAlgNode Configured! --> values received: " << std::endl;
   std::cout << "sensor_height               =                   " << this->alg_.lidar_configuration_.sensor_height
       << std::endl;
@@ -224,7 +265,6 @@ void GroundSegmentationAlgNode::node_config_update(Config &config, uint32_t leve
   std::cout << "distance_to_virtual_references_in_shadow_area = "
       << this->alg_.filtering_configuration_.distance_to_virtual_references_in_shadow_area << std::endl;
 
-
   std::cout << "propagation_additive_noise in m/m  =          "
       << this->alg_.filtering_configuration_.propagation_z_additive_noise << std::endl;
   std::cout << "propagation_additive_noise in deg/m  =          "
@@ -247,20 +287,16 @@ void GroundSegmentationAlgNode::node_config_update(Config &config, uint32_t leve
   std::cout << "ground_reference_search_resolution =            "
       << this->alg_.filtering_configuration_.ground_reference_search_resolution * 180.0 / M_PI << std::endl;
 
-  std::cout << "robot_width    = "
-      << this->alg_.filtering_configuration_.robot_width << std::endl;
+  std::cout << "robot_width    = " << this->alg_.filtering_configuration_.robot_width << std::endl;
 
-  std::cout << "robot_height    = "
-      << this->alg_.filtering_configuration_.robot_height << std::endl;
+  std::cout << "robot_height    = " << this->alg_.filtering_configuration_.robot_height << std::endl;
 
-  std::cout << "safety_margin  = "
-      << this->alg_.filtering_configuration_.safety_margin << std::endl;
+  std::cout << "safety_margin  = " << this->alg_.filtering_configuration_.safety_margin << std::endl;
 
-  std::cout << "measure_performance  = "
-      << this->alg_.filtering_configuration_.measure_performance << std::endl;
+  std::cout << "measure_performance  = " << this->alg_.filtering_configuration_.measure_performance << std::endl;
 
-  std::cout << "show_dense_reconstruction  = "
-      << this->alg_.filtering_configuration_.show_dense_reconstruction << std::endl;
+  std::cout << "show_dense_reconstruction  = " << this->alg_.filtering_configuration_.show_dense_reconstruction
+      << std::endl;
 }
 
 void GroundSegmentationAlgNode::addNodeDiagnostics(void)
@@ -270,5 +306,6 @@ void GroundSegmentationAlgNode::addNodeDiagnostics(void)
 /* main function */
 int main(int argc, char *argv[])
 {
-  return algorithm_base::main<GroundSegmentationAlgNode>(argc, argv, "two_segmented_inputs_vehicle_detector_alg_node");
+  return algorithm_base::main < GroundSegmentationAlgNode
+      > (argc, argv, "two_segmented_inputs_vehicle_detector_alg_node");
 }