diff --git a/cfg/GroundSegmentation.cfg b/cfg/GroundSegmentation.cfg index f789cceb1bd166b734d3260bbdcf7f232f6a2688..cefcd5eb0e8f31dcecfbebdd34c42715b0b5d8b0 100644 --- a/cfg/GroundSegmentation.cfg +++ b/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); diff --git a/include/ground_segmentation_alg_node.h b/include/ground_segmentation_alg_node.h index 86066fea43500775b542610015b78acb25a11f00..d81b8065145095c29f44be4379061d8a60051cd3 100644 --- a/include/ground_segmentation_alg_node.h +++ b/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_; diff --git a/src/ground_segmentation_alg_node.cpp b/src/ground_segmentation_alg_node.cpp index e2c28bc305e7b31393422cfbbeac4406096b93cf..b47c7a5906a877459a07288a51e38cb768930dc6 100644 --- a/src/ground_segmentation_alg_node.cpp +++ b/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"); }