From 0ad928f6a1b707f62ab2e06c0abf0df424af9d3e Mon Sep 17 00:00:00 2001 From: maxlein Date: Mon, 2 Dec 2019 10:38:01 +0100 Subject: [PATCH 1/3] Check if parameters exist already before declaring them --- src/slam_mapper.cpp | 92 +++++++++++++++++++++++++++------------------ 1 file changed, 56 insertions(+), 36 deletions(-) diff --git a/src/slam_mapper.cpp b/src/slam_mapper.cpp index 56dd0da22..d14a12093 100644 --- a/src/slam_mapper.cpp +++ b/src/slam_mapper.cpp @@ -88,115 +88,125 @@ void SMapper::configure(const rclcpp::Node::SharedPtr & node) /*****************************************************************************/ { bool use_scan_matching = true; - if(node->declare_parameter("use_scan_matching", use_scan_matching)) + if(not node->has_parameter("use_scan_matching") && + node->declare_parameter("use_scan_matching", use_scan_matching)) { mapper_->setParamUseScanMatching(use_scan_matching); } bool use_scan_barycenter = true; - if(node->declare_parameter("use_scan_barycenter", use_scan_barycenter)) + if(not node->has_parameter("use_scan_barycenter") && + node->declare_parameter("use_scan_barycenter", use_scan_barycenter)) { mapper_->setParamUseScanBarycenter(use_scan_barycenter); } double minimum_travel_distance = 0.5; - if(node->declare_parameter("minimum_travel_distance", + if(not node->has_parameter("minimum_travel_distance") && + node->declare_parameter("minimum_travel_distance", minimum_travel_distance)) { mapper_->setParamMinimumTravelDistance(minimum_travel_distance); } double minimum_travel_heading = 0.5; - if(node->declare_parameter("minimum_travel_heading", + if(not node->has_parameter("minimum_travel_heading") && + node->declare_parameter("minimum_travel_heading", minimum_travel_heading)) { mapper_->setParamMinimumTravelHeading(minimum_travel_heading); } int scan_buffer_size = 10; - if(node->declare_parameter("scan_buffer_size", scan_buffer_size)) + if(not node->has_parameter("scan_buffer_size") && + node->declare_parameter("scan_buffer_size", scan_buffer_size)) { mapper_->setParamScanBufferSize(scan_buffer_size); } double scan_buffer_maximum_scan_distance = 10; - if(node->declare_parameter("scan_buffer_maximum_scan_distance", + if(not node->has_parameter("scan_buffer_maximum_scan_distance") && + node->declare_parameter("scan_buffer_maximum_scan_distance", scan_buffer_maximum_scan_distance)) { mapper_->setParamScanBufferMaximumScanDistance(scan_buffer_maximum_scan_distance); } double link_match_minimum_response_fine = 0.1; - if(node->declare_parameter("link_match_minimum_response_fine", + if(not node->has_parameter("link_match_minimum_response_fine") && + node->declare_parameter("link_match_minimum_response_fine", link_match_minimum_response_fine)) { mapper_->setParamLinkMatchMinimumResponseFine(link_match_minimum_response_fine); } double link_scan_maximum_distance = 1.5; - if(node->declare_parameter("link_scan_maximum_distance", link_scan_maximum_distance)) + if(not node->has_parameter("link_scan_maximum_distance") && + node->declare_parameter("link_scan_maximum_distance", link_scan_maximum_distance)) { mapper_->setParamLinkScanMaximumDistance(link_scan_maximum_distance); } double loop_search_maximum_distance = 3.0; - if(node->declare_parameter("loop_search_maximum_distance", loop_search_maximum_distance)) + if(not node->has_parameter("loop_search_maximum_distance") && + node->declare_parameter("loop_search_maximum_distance", loop_search_maximum_distance)) { mapper_->setParamLoopSearchMaximumDistance(loop_search_maximum_distance); } bool do_loop_closing = true; - if(node->declare_parameter("do_loop_closing", do_loop_closing)) + if(not node->has_parameter("do_loop_closing") && + node->declare_parameter("do_loop_closing", do_loop_closing)) { mapper_->setParamDoLoopClosing(do_loop_closing); } int loop_match_minimum_chain_size = 10; - if(node->declare_parameter("loop_match_minimum_chain_size", - loop_match_minimum_chain_size)) + if(not node->has_parameter("loop_match_minimum_chain_size") && + node->declare_parameter("loop_match_minimum_chain_size", loop_match_minimum_chain_size)) { mapper_->setParamLoopMatchMinimumChainSize(loop_match_minimum_chain_size); } double loop_match_maximum_variance_coarse = 3.0; - if(node->declare_parameter("loop_match_maximum_variance_coarse", - loop_match_maximum_variance_coarse)) + if(not node->has_parameter("loop_match_maximum_variance_coarse") && + node->declare_parameter("loop_match_maximum_variance_coarse", loop_match_maximum_variance_coarse)) { mapper_->setParamLoopMatchMaximumVarianceCoarse(loop_match_maximum_variance_coarse); } double loop_match_minimum_response_coarse = 0.35; - if(node->declare_parameter("loop_match_minimum_response_coarse", - loop_match_minimum_response_coarse)) + if(not node->has_parameter("loop_match_minimum_response_coarse") && + node->declare_parameter("loop_match_minimum_response_coarse", loop_match_minimum_response_coarse)) { mapper_->setParamLoopMatchMinimumResponseCoarse(loop_match_minimum_response_coarse); } double loop_match_minimum_response_fine = 0.45; - if(node->declare_parameter("loop_match_minimum_response_fine", - loop_match_minimum_response_fine)) + if(not node->has_parameter("loop_match_minimum_response_fine") && + node->declare_parameter("loop_match_minimum_response_fine", loop_match_minimum_response_fine)) { mapper_->setParamLoopMatchMinimumResponseFine(loop_match_minimum_response_fine); } // Setting Correlation Parameters double correlation_search_space_dimension = 0.5; - if(node->declare_parameter("correlation_search_space_dimension", - correlation_search_space_dimension)) + if(not node->has_parameter("correlation_search_space_dimension") && + node->declare_parameter("correlation_search_space_dimension", correlation_search_space_dimension)) { mapper_->setParamCorrelationSearchSpaceDimension(correlation_search_space_dimension); } double correlation_search_space_resolution = 0.01; - if(node->declare_parameter("correlation_search_space_resolution", - correlation_search_space_resolution)) + if(not node->has_parameter("correlation_search_space_resolution") && + node->declare_parameter("correlation_search_space_resolution", correlation_search_space_resolution)) { mapper_->setParamCorrelationSearchSpaceResolution(correlation_search_space_resolution); } double correlation_search_space_smear_deviation = 0.1; - if(node->declare_parameter("correlation_search_space_smear_deviation", - correlation_search_space_smear_deviation)) + if(not node->has_parameter("correlation_search_space_smear_deviation") && + node->declare_parameter("correlation_search_space_smear_deviation", correlation_search_space_smear_deviation)) { mapper_->setParamCorrelationSearchSpaceSmearDeviation( correlation_search_space_smear_deviation); @@ -204,69 +214,79 @@ void SMapper::configure(const rclcpp::Node::SharedPtr & node) // Setting Correlation Parameters, Loop Closure Parameters double loop_search_space_dimension = 8.0; - if(node->declare_parameter("loop_search_space_dimension", loop_search_space_dimension)) + if(not node->has_parameter("loop_search_space_dimension") && + node->declare_parameter("loop_search_space_dimension", loop_search_space_dimension)) { mapper_->setParamLoopSearchSpaceDimension(loop_search_space_dimension); } double loop_search_space_resolution = 0.05; - if(node->declare_parameter("loop_search_space_resolution", loop_search_space_resolution)) + if(not node->has_parameter("loop_search_space_resolution") && + node->declare_parameter("loop_search_space_resolution", loop_search_space_resolution)) { mapper_->setParamLoopSearchSpaceResolution(loop_search_space_resolution); } double loop_search_space_smear_deviation = 0.03; - if(node->declare_parameter("loop_search_space_smear_deviation", - loop_search_space_smear_deviation)) + if(not node->has_parameter("loop_search_space_smear_deviation") && + node->declare_parameter("loop_search_space_smear_deviation", loop_search_space_smear_deviation)) { mapper_->setParamLoopSearchSpaceSmearDeviation(loop_search_space_smear_deviation); } // Setting Scan Matcher Parameters double distance_variance_penalty = 0.5; - if(node->declare_parameter("distance_variance_penalty", distance_variance_penalty)) + if(not node->has_parameter("distance_variance_penalty") && + node->declare_parameter("distance_variance_penalty", distance_variance_penalty)) { mapper_->setParamDistanceVariancePenalty(distance_variance_penalty); } double angle_variance_penalty = 1.0; - if(node->declare_parameter("angle_variance_penalty", angle_variance_penalty)) + if(not node->has_parameter("angle_variance_penalty") && + node->declare_parameter("angle_variance_penalty", angle_variance_penalty)) { mapper_->setParamAngleVariancePenalty(angle_variance_penalty); } double fine_search_angle_offset = 0.00349; - if(node->declare_parameter("fine_search_angle_offset", fine_search_angle_offset)) + if(not node->has_parameter("fine_search_angle_offset") && + node->declare_parameter("fine_search_angle_offset", fine_search_angle_offset)) { mapper_->setParamFineSearchAngleOffset(fine_search_angle_offset); } double coarse_search_angle_offset = 0.349; - if(node->declare_parameter("coarse_search_angle_offset", coarse_search_angle_offset)) + if(not node->has_parameter("coarse_search_angle_offset") && + node->declare_parameter("coarse_search_angle_offset", coarse_search_angle_offset)) { mapper_->setParamCoarseSearchAngleOffset(coarse_search_angle_offset); } double coarse_angle_resolution = 0.0349; - if(node->declare_parameter("coarse_angle_resolution", coarse_angle_resolution)) + if(not node->has_parameter("coarse_angle_resolution") && + node->declare_parameter("coarse_angle_resolution", coarse_angle_resolution)) { mapper_->setParamCoarseAngleResolution(coarse_angle_resolution); } double minimum_angle_penalty = 0.9; - if(node->declare_parameter("minimum_angle_penalty", minimum_angle_penalty)) + if(not node->has_parameter("minimum_angle_penalty") && + node->declare_parameter("minimum_angle_penalty", minimum_angle_penalty)) { mapper_->setParamMinimumAnglePenalty(minimum_angle_penalty); } double minimum_distance_penalty = 0.05; - if(node->declare_parameter("minimum_distance_penalty", minimum_distance_penalty)) + if(not node->has_parameter("minimum_distance_penalty") && + node->declare_parameter("minimum_distance_penalty", minimum_distance_penalty)) { mapper_->setParamMinimumDistancePenalty(minimum_distance_penalty); } bool use_response_expansion = true; - if(node->declare_parameter("use_response_expansion", use_response_expansion)) + if(not node->has_parameter("use_response_expansion") && + node->declare_parameter("use_response_expansion", use_response_expansion)) { mapper_->setParamUseResponseExpansion(use_response_expansion); } From 0a59bc93c74df64f8741d7002a7d0e24fc923666 Mon Sep 17 00:00:00 2001 From: Max Leingartner Date: Mon, 2 Dec 2019 18:29:58 +0100 Subject: [PATCH 2/3] fixed coding style --- src/slam_mapper.cpp | 56 ++++++++++++++++++++++----------------------- 1 file changed, 28 insertions(+), 28 deletions(-) diff --git a/src/slam_mapper.cpp b/src/slam_mapper.cpp index d14a12093..3a1b51e18 100644 --- a/src/slam_mapper.cpp +++ b/src/slam_mapper.cpp @@ -88,21 +88,21 @@ void SMapper::configure(const rclcpp::Node::SharedPtr & node) /*****************************************************************************/ { bool use_scan_matching = true; - if(not node->has_parameter("use_scan_matching") && + if(!node->has_parameter("use_scan_matching") && node->declare_parameter("use_scan_matching", use_scan_matching)) { mapper_->setParamUseScanMatching(use_scan_matching); } bool use_scan_barycenter = true; - if(not node->has_parameter("use_scan_barycenter") && + if(!node->has_parameter("use_scan_barycenter") && node->declare_parameter("use_scan_barycenter", use_scan_barycenter)) { mapper_->setParamUseScanBarycenter(use_scan_barycenter); } double minimum_travel_distance = 0.5; - if(not node->has_parameter("minimum_travel_distance") && + if(!node->has_parameter("minimum_travel_distance") && node->declare_parameter("minimum_travel_distance", minimum_travel_distance)) { @@ -110,7 +110,7 @@ void SMapper::configure(const rclcpp::Node::SharedPtr & node) } double minimum_travel_heading = 0.5; - if(not node->has_parameter("minimum_travel_heading") && + if(!node->has_parameter("minimum_travel_heading") && node->declare_parameter("minimum_travel_heading", minimum_travel_heading)) { @@ -118,14 +118,14 @@ void SMapper::configure(const rclcpp::Node::SharedPtr & node) } int scan_buffer_size = 10; - if(not node->has_parameter("scan_buffer_size") && + if(!node->has_parameter("scan_buffer_size") && node->declare_parameter("scan_buffer_size", scan_buffer_size)) { mapper_->setParamScanBufferSize(scan_buffer_size); } double scan_buffer_maximum_scan_distance = 10; - if(not node->has_parameter("scan_buffer_maximum_scan_distance") && + if(!node->has_parameter("scan_buffer_maximum_scan_distance") && node->declare_parameter("scan_buffer_maximum_scan_distance", scan_buffer_maximum_scan_distance)) { @@ -133,7 +133,7 @@ void SMapper::configure(const rclcpp::Node::SharedPtr & node) } double link_match_minimum_response_fine = 0.1; - if(not node->has_parameter("link_match_minimum_response_fine") && + if(!node->has_parameter("link_match_minimum_response_fine") && node->declare_parameter("link_match_minimum_response_fine", link_match_minimum_response_fine)) { @@ -141,49 +141,49 @@ void SMapper::configure(const rclcpp::Node::SharedPtr & node) } double link_scan_maximum_distance = 1.5; - if(not node->has_parameter("link_scan_maximum_distance") && + if(!node->has_parameter("link_scan_maximum_distance") && node->declare_parameter("link_scan_maximum_distance", link_scan_maximum_distance)) { mapper_->setParamLinkScanMaximumDistance(link_scan_maximum_distance); } double loop_search_maximum_distance = 3.0; - if(not node->has_parameter("loop_search_maximum_distance") && + if(!node->has_parameter("loop_search_maximum_distance") && node->declare_parameter("loop_search_maximum_distance", loop_search_maximum_distance)) { mapper_->setParamLoopSearchMaximumDistance(loop_search_maximum_distance); } bool do_loop_closing = true; - if(not node->has_parameter("do_loop_closing") && + if(!node->has_parameter("do_loop_closing") && node->declare_parameter("do_loop_closing", do_loop_closing)) { mapper_->setParamDoLoopClosing(do_loop_closing); } int loop_match_minimum_chain_size = 10; - if(not node->has_parameter("loop_match_minimum_chain_size") && + if(!node->has_parameter("loop_match_minimum_chain_size") && node->declare_parameter("loop_match_minimum_chain_size", loop_match_minimum_chain_size)) { mapper_->setParamLoopMatchMinimumChainSize(loop_match_minimum_chain_size); } double loop_match_maximum_variance_coarse = 3.0; - if(not node->has_parameter("loop_match_maximum_variance_coarse") && + if(!node->has_parameter("loop_match_maximum_variance_coarse") && node->declare_parameter("loop_match_maximum_variance_coarse", loop_match_maximum_variance_coarse)) { mapper_->setParamLoopMatchMaximumVarianceCoarse(loop_match_maximum_variance_coarse); } double loop_match_minimum_response_coarse = 0.35; - if(not node->has_parameter("loop_match_minimum_response_coarse") && + if(!node->has_parameter("loop_match_minimum_response_coarse") && node->declare_parameter("loop_match_minimum_response_coarse", loop_match_minimum_response_coarse)) { mapper_->setParamLoopMatchMinimumResponseCoarse(loop_match_minimum_response_coarse); } double loop_match_minimum_response_fine = 0.45; - if(not node->has_parameter("loop_match_minimum_response_fine") && + if(!node->has_parameter("loop_match_minimum_response_fine") && node->declare_parameter("loop_match_minimum_response_fine", loop_match_minimum_response_fine)) { mapper_->setParamLoopMatchMinimumResponseFine(loop_match_minimum_response_fine); @@ -191,21 +191,21 @@ void SMapper::configure(const rclcpp::Node::SharedPtr & node) // Setting Correlation Parameters double correlation_search_space_dimension = 0.5; - if(not node->has_parameter("correlation_search_space_dimension") && + if(!node->has_parameter("correlation_search_space_dimension") && node->declare_parameter("correlation_search_space_dimension", correlation_search_space_dimension)) { mapper_->setParamCorrelationSearchSpaceDimension(correlation_search_space_dimension); } double correlation_search_space_resolution = 0.01; - if(not node->has_parameter("correlation_search_space_resolution") && + if(!node->has_parameter("correlation_search_space_resolution") && node->declare_parameter("correlation_search_space_resolution", correlation_search_space_resolution)) { mapper_->setParamCorrelationSearchSpaceResolution(correlation_search_space_resolution); } double correlation_search_space_smear_deviation = 0.1; - if(not node->has_parameter("correlation_search_space_smear_deviation") && + if(!node->has_parameter("correlation_search_space_smear_deviation") && node->declare_parameter("correlation_search_space_smear_deviation", correlation_search_space_smear_deviation)) { mapper_->setParamCorrelationSearchSpaceSmearDeviation( @@ -214,21 +214,21 @@ void SMapper::configure(const rclcpp::Node::SharedPtr & node) // Setting Correlation Parameters, Loop Closure Parameters double loop_search_space_dimension = 8.0; - if(not node->has_parameter("loop_search_space_dimension") && + if(!node->has_parameter("loop_search_space_dimension") && node->declare_parameter("loop_search_space_dimension", loop_search_space_dimension)) { mapper_->setParamLoopSearchSpaceDimension(loop_search_space_dimension); } double loop_search_space_resolution = 0.05; - if(not node->has_parameter("loop_search_space_resolution") && + if(!node->has_parameter("loop_search_space_resolution") && node->declare_parameter("loop_search_space_resolution", loop_search_space_resolution)) { mapper_->setParamLoopSearchSpaceResolution(loop_search_space_resolution); } double loop_search_space_smear_deviation = 0.03; - if(not node->has_parameter("loop_search_space_smear_deviation") && + if(!node->has_parameter("loop_search_space_smear_deviation") && node->declare_parameter("loop_search_space_smear_deviation", loop_search_space_smear_deviation)) { mapper_->setParamLoopSearchSpaceSmearDeviation(loop_search_space_smear_deviation); @@ -236,56 +236,56 @@ void SMapper::configure(const rclcpp::Node::SharedPtr & node) // Setting Scan Matcher Parameters double distance_variance_penalty = 0.5; - if(not node->has_parameter("distance_variance_penalty") && + if(!node->has_parameter("distance_variance_penalty") && node->declare_parameter("distance_variance_penalty", distance_variance_penalty)) { mapper_->setParamDistanceVariancePenalty(distance_variance_penalty); } double angle_variance_penalty = 1.0; - if(not node->has_parameter("angle_variance_penalty") && + if(!node->has_parameter("angle_variance_penalty") && node->declare_parameter("angle_variance_penalty", angle_variance_penalty)) { mapper_->setParamAngleVariancePenalty(angle_variance_penalty); } double fine_search_angle_offset = 0.00349; - if(not node->has_parameter("fine_search_angle_offset") && + if(!node->has_parameter("fine_search_angle_offset") && node->declare_parameter("fine_search_angle_offset", fine_search_angle_offset)) { mapper_->setParamFineSearchAngleOffset(fine_search_angle_offset); } double coarse_search_angle_offset = 0.349; - if(not node->has_parameter("coarse_search_angle_offset") && + if(!node->has_parameter("coarse_search_angle_offset") && node->declare_parameter("coarse_search_angle_offset", coarse_search_angle_offset)) { mapper_->setParamCoarseSearchAngleOffset(coarse_search_angle_offset); } double coarse_angle_resolution = 0.0349; - if(not node->has_parameter("coarse_angle_resolution") && + if(!node->has_parameter("coarse_angle_resolution") && node->declare_parameter("coarse_angle_resolution", coarse_angle_resolution)) { mapper_->setParamCoarseAngleResolution(coarse_angle_resolution); } double minimum_angle_penalty = 0.9; - if(not node->has_parameter("minimum_angle_penalty") && + if(!node->has_parameter("minimum_angle_penalty") && node->declare_parameter("minimum_angle_penalty", minimum_angle_penalty)) { mapper_->setParamMinimumAnglePenalty(minimum_angle_penalty); } double minimum_distance_penalty = 0.05; - if(not node->has_parameter("minimum_distance_penalty") && + if(!node->has_parameter("minimum_distance_penalty") && node->declare_parameter("minimum_distance_penalty", minimum_distance_penalty)) { mapper_->setParamMinimumDistancePenalty(minimum_distance_penalty); } bool use_response_expansion = true; - if(not node->has_parameter("use_response_expansion") && + if(!node->has_parameter("use_response_expansion") && node->declare_parameter("use_response_expansion", use_response_expansion)) { mapper_->setParamUseResponseExpansion(use_response_expansion); From 96cea47b01c1c2cfbe027e56e1898af142c1624e Mon Sep 17 00:00:00 2001 From: Max Leingartner Date: Wed, 15 Jan 2020 17:54:53 +0100 Subject: [PATCH 3/3] changed message depth of scan filter sub to 1 --- src/slam_toolbox_common.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/slam_toolbox_common.cpp b/src/slam_toolbox_common.cpp index 352e16e48..4e8b91ffd 100644 --- a/src/slam_toolbox_common.cpp +++ b/src/slam_toolbox_common.cpp @@ -211,7 +211,7 @@ void SlamToolbox::setROSInterfaces() shared_from_this().get(), scan_topic_, rmw_qos_profile_sensor_data); scan_filter_ = std::make_unique >( - *scan_filter_sub_, *tf_, odom_frame_, 10, shared_from_this()); + *scan_filter_sub_, *tf_, odom_frame_, 1, shared_from_this()); scan_filter_->registerCallback( std::bind(&SlamToolbox::laserCallback, this, std::placeholders::_1)); }