Skip to content

Commit

Permalink
Dynamic reconfigure for AMCL (ros-navigation#2932)
Browse files Browse the repository at this point in the history 
* not finished

* update

* satisfying linters

* add mutex

* fix mistake

* dealing each parameters

* segregating variables and instantiating the objects correspondingly

* remove unwanted mutex

* reconfigure for map topic

* review update

* careful updation of min and max particles

* resetting and reinitilaizing the laser scanner and message filters

* fixing the mutex
  • Loading branch information
@padhupradheep @dimaxano
padhupradheep authored and dimaxano committed Jan 4, 2023
1 parent ce0cb55 commit 1f5fcf9
Show file tree
Hide file tree
Showing 2 changed files with 221 additions and 0 deletions.
10 changes: 10 additions & 0 deletions nav2_amcl/include/nav2_amcl/amcl_node.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -95,6 +95,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
211 changes: 211 additions & 0 deletions nav2_amcl/src/amcl_node.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -54,6 +54,7 @@
#include "portable_utils.h"

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

namespace nav2_amcl
Expand Down Expand Up @@ -331,6 +332,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 @@ -348,6 +356,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 @@ -1308,6 +1319,206 @@ 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(mutex_);
rcl_interfaces::msg::SetParametersResult result;
double save_pose_rate;
double tmp_tol;

int max_particles = max_particles_;
int min_particles = min_particles_;

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();
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.");
// sticking to the old values
max_particles_ = max_particles;
min_particles_ = min_particles;
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_connection_.disconnect();
laser_scan_sub_.reset();

initMessageFilters();
}

// 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

0 comments on commit 1f5fcf9

Please sign in to comment.