Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

changed message depth of scan filter sub to 1 #150

Merged
merged 3 commits into from
Jan 15, 2020
Merged

changed message depth of scan filter sub to 1 #150

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
3 commits
Select commit Hold shift + click to select a range
0ad928f
Check if parameters exist already before declaring them
maxlein Dec 2, 2019
0a59bc9
fixed coding style
maxlein Dec 2, 2019
96cea47
changed message depth of scan filter sub to 1
maxlein Jan 15, 2020
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
92 changes: 56 additions & 36 deletions src/slam_mapper.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -88,185 +88,205 @@ void SMapper::configure(const rclcpp::Node::SharedPtr & node)
/*****************************************************************************/
{
bool use_scan_matching = true;
if(node->declare_parameter("use_scan_matching", 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(node->declare_parameter("use_scan_barycenter", 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(node->declare_parameter("minimum_travel_distance",
if(!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(!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(!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(!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(!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(!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(!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(!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(!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(!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(!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(!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(!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(!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(!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);
}

// 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(!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(!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(!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(!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(!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(!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(!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(!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(!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(!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(!node->has_parameter("use_response_expansion") &&
node->declare_parameter("use_response_expansion", use_response_expansion))
{
mapper_->setParamUseResponseExpansion(use_response_expansion);
}
Expand Down
2 changes: 1 addition & 1 deletion src/slam_toolbox_common.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -211,7 +211,7 @@ void SlamToolbox::setROSInterfaces()
shared_from_this().get(), scan_topic_, rmw_qos_profile_sensor_data);
scan_filter_ =
std::make_unique<tf2_ros::MessageFilter<sensor_msgs::msg::LaserScan> >(
*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));
}
Expand Down