Skip to content

Dynamic reconfigure for AMCL #2932

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

Merged
merged 14 commits into from
May 9, 2022
Merged

Dynamic reconfigure for AMCL #2932

Show file tree
Hide file tree
Changes from 10 commits
Commits
Show all changes
14 commits
Select commit Hold shift + click to select a range
9eaeee0
not finished
padhupradheep Apr 14, 2022
6bda2a3
update
padhupradheep May 3, 2022
6325f02
satisfying linters
padhupradheep May 3, 2022
350fd15
add mutex
padhupradheep May 3, 2022
e198f6d
fix mistake
padhupradheep May 3, 2022
8bd6175
dealing each parameters
padhupradheep May 4, 2022
e379d7b
segregating variables and instantiating the objects correspondingly
padhupradheep May 4, 2022
4fcd022
remove unwanted mutex
padhupradheep May 4, 2022
b2896ab
reconfigure for map topic
padhupradheep May 5, 2022
f78072d
review update
padhupradheep May 6, 2022
4bd6b65
careful updation of min and max particles
padhupradheep May 9, 2022
108f8b9
resetting and reinitilaizing the laser scanner and message filters
padhupradheep May 9, 2022
855691d
Merge branch 'main' of github.com:ros-planning/navigation2 into dynam…
padhupradheep May 9, 2022
befa799
fixing the mutex
padhupradheep May 9, 2022
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
10 changes: 10 additions & 0 deletions nav2_amcl/include/nav2_amcl/amcl_node.hpp
View file
Open in desktop
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
View file
Open in desktop
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();
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_;
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>>(
*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