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.

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

Dynamic reconfigure for AMCL #2932

Merged
merged 14 commits into from
May 9, 2022
10 changes: 10 additions & 0 deletions nav2_amcl/include/nav2_amcl/amcl_node.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -91,6 +91,16 @@ class AmclNode : public nav2_util::LifecycleNode
*/
nav2_util::CallbackReturn on_shutdown(const rclcpp_lifecycle::State & state) override;

/**
* @brief Callback executed when a parameter change is detected
* @param event ParameterEvent message
*/
rcl_interfaces::msg::SetParametersResult
dynamicParametersCallback(std::vector<rclcpp::Parameter> parameters);

// Dynamic parameters handler
rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr dyn_params_handler_;

// Since the sensor data from gazebo or the robot is not lifecycle enabled, we won't
// respond until we're in the active state
std::atomic<bool> active_{false};
Expand Down
210 changes: 210 additions & 0 deletions nav2_amcl/src/amcl_node.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -51,6 +51,7 @@
#include "nav2_amcl/portable_utils.hpp"

using namespace std::placeholders;
using rcl_interfaces::msg::ParameterType;
using namespace std::chrono_literals;

namespace nav2_amcl
Expand Down Expand Up @@ -278,6 +279,13 @@ AmclNode::on_activate(const rclcpp_lifecycle::State & /*state*/)
handleInitialPose(last_published_pose_);
}

auto node = shared_from_this();
// Add callback for dynamic parameters
dyn_params_handler_ = node->add_on_set_parameters_callback(
std::bind(
&AmclNode::dynamicParametersCallback,
this, std::placeholders::_1));

// create bond connection
createBond();

Expand All @@ -295,6 +303,9 @@ AmclNode::on_deactivate(const rclcpp_lifecycle::State & /*state*/)
pose_pub_->on_deactivate();
particle_cloud_pub_->on_deactivate();

// reset dynamic parameter handler
dyn_params_handler_.reset();

// destroy bond connection
destroyBond();

Expand Down Expand Up @@ -1113,6 +1124,205 @@ AmclNode::initParameters()
}
}

/**
* @brief Callback executed when a parameter change is detected
* @param event ParameterEvent message
*/
rcl_interfaces::msg::SetParametersResult
AmclNode::dynamicParametersCallback(
std::vector<rclcpp::Parameter> parameters)
{
std::lock_guard<std::recursive_mutex> cfl(configuration_mutex_);
rcl_interfaces::msg::SetParametersResult result;
double save_pose_rate;
double tmp_tol;

bool reinit_pf = false;
bool reinit_odom = false;
bool reinit_laser = false;
bool reinit_map = false;

for (auto parameter : parameters) {
const auto & param_type = parameter.get_type();
const auto & param_name = parameter.get_name();

if (param_type == ParameterType::PARAMETER_DOUBLE) {
if (param_name == "alpha1") {
alpha1_ = parameter.as_double();
reinit_odom = true;
} else if (param_name == "alpha2") {
alpha2_ = parameter.as_double();
reinit_odom = true;
} else if (param_name == "alpha3") {
alpha3_ = parameter.as_double();
reinit_odom = true;
} else if (param_name == "alpha4") {
alpha4_ = parameter.as_double();
reinit_odom = true;
} else if (param_name == "alpha5") {
alpha5_ = parameter.as_double();
reinit_odom = true;
} else if (param_name == "beam_skip_distance") {
beam_skip_distance_ = parameter.as_double();
reinit_laser = true;
} else if (param_name == "beam_skip_error_threshold") {
beam_skip_error_threshold_ = parameter.as_double();
reinit_laser = true;
} else if (param_name == "beam_skip_threshold") {
beam_skip_threshold_ = parameter.as_double();
reinit_laser = true;
} else if (param_name == "lambda_short") {
lambda_short_ = parameter.as_double();
reinit_laser = true;
} else if (param_name == "laser_likelihood_max_dist") {
laser_likelihood_max_dist_ = parameter.as_double();
reinit_laser = true;
} else if (param_name == "laser_max_range") {
laser_max_range_ = parameter.as_double();
reinit_laser = true;
} else if (param_name == "laser_min_range") {
laser_min_range_ = parameter.as_double();
reinit_laser = true;
} else if (param_name == "pf_err") {
pf_err_ = parameter.as_double();
reinit_pf = true;
} else if (param_name == "pf_z") {
pf_z_ = parameter.as_double();
reinit_pf = true;
} else if (param_name == "recovery_alpha_fast") {
alpha_fast_ = parameter.as_double();
reinit_pf = true;
} else if (param_name == "recovery_alpha_slow") {
alpha_slow_ = parameter.as_double();
reinit_pf = true;
} else if (param_name == "save_pose_rate") {
save_pose_rate = parameter.as_double();
save_pose_period_ = tf2::durationFromSec(1.0 / save_pose_rate);
} else if (param_name == "sigma_hit") {
sigma_hit_ = parameter.as_double();
reinit_laser = true;
} else if (param_name == "transform_tolerance") {
tmp_tol = parameter.as_double();
transform_tolerance_ = tf2::durationFromSec(tmp_tol);
reinit_laser = true;
} else if (param_name == "update_min_a") {
a_thresh_ = parameter.as_double();
} else if (param_name == "update_min_d") {
d_thresh_ = parameter.as_double();
} else if (param_name == "z_hit") {
z_hit_ = parameter.as_double();
reinit_laser = true;
} else if (param_name == "z_max") {
z_max_ = parameter.as_double();
reinit_laser = true;
} else if (param_name == "z_rand") {
z_rand_ = parameter.as_double();
reinit_laser = true;
} else if (param_name == "z_short") {
z_short_ = parameter.as_double();
reinit_laser = true;
}
} else if (param_type == ParameterType::PARAMETER_STRING) {
if (param_name == "base_frame_id") {
base_frame_id_ = parameter.as_string();
} else if (param_name == "global_frame_id") {
global_frame_id_ = parameter.as_string();
} else if (param_name == "map_topic") {
map_topic_ = parameter.as_string();
SteveMacenski marked this conversation as resolved.
Show resolved Hide resolved
reinit_map = true;
} else if (param_name == "laser_model_type") {
sensor_model_type_ = parameter.as_string();
reinit_laser = true;
} else if (param_name == "odom_frame_id") {
odom_frame_id_ = parameter.as_string();
reinit_laser = true;
} else if (param_name == "scan_topic") {
scan_topic_ = parameter.as_string();
reinit_laser = true;
} else if (param_name == "robot_model_type") {
robot_model_type_ = parameter.as_string();
reinit_odom = true;
}
} else if (param_type == ParameterType::PARAMETER_BOOL) {
if (param_name == "do_beamskip") {
do_beamskip_ = parameter.as_bool();
reinit_laser = true;
} else if (param_name == "tf_broadcast") {
tf_broadcast_ = parameter.as_bool();
} else if (param_name == "set_initial_pose") {
set_initial_pose_ = parameter.as_bool();
} else if (param_name == "first_map_only") {
first_map_only_ = parameter.as_bool();
}
} else if (param_type == ParameterType::PARAMETER_INTEGER) {
if (param_name == "max_beams") {
max_beams_ = parameter.as_int();
reinit_laser = true;
} else if (param_name == "max_particles") {
max_particles_ = parameter.as_int();
reinit_pf = true;
} else if (param_name == "min_particles") {
min_particles_ = parameter.as_int();
reinit_pf = true;
} else if (param_name == "resample_interval") {
resample_interval_ = parameter.as_int();
}
}
}

// Checking if the minimum particles is greater than max_particles_
if (min_particles_ > max_particles_) {
RCLCPP_ERROR(
this->get_logger(),
"You've set min_particles to be greater than max particles,"
" this isn't allowed.");
max_particles_ = min_particles_;
SteveMacenski marked this conversation as resolved.
Show resolved Hide resolved
result.successful = false;
return result;
}

// Re-initialize the particle filter
if (reinit_pf) {
if (pf_ != NULL) {
pf_free(pf_);
pf_ = NULL;
}
initParticleFilter();
}

// Re-initialize the odometry
if (reinit_odom) {
motion_model_.reset();
initOdometry();
}

// Re-initialize the lasers and it's filters
if (reinit_laser) {
lasers_.clear();
lasers_update_.clear();
frame_to_laser_.clear();

laser_scan_filter_ = std::make_unique<tf2_ros::MessageFilter<sensor_msgs::msg::LaserScan>>(
SteveMacenski marked this conversation as resolved.
Show resolved Hide resolved
*laser_scan_sub_, *tf_buffer_, odom_frame_id_, 10, rclcpp_node_, transform_tolerance_);

laser_scan_connection_ = laser_scan_filter_->registerCallback(
std::bind(
&AmclNode::laserReceived,
this, std::placeholders::_1));
}

// Re-initialize the map
if (reinit_map) {
map_sub_.reset();
map_sub_ = create_subscription<nav_msgs::msg::OccupancyGrid>(
map_topic_, rclcpp::QoS(rclcpp::KeepLast(1)).transient_local().reliable(),
std::bind(&AmclNode::mapReceived, this, std::placeholders::_1));
}

result.successful = true;
return result;
}

void
AmclNode::mapReceived(const nav_msgs::msg::OccupancyGrid::SharedPtr msg)
{
Expand Down