Skip to content

Commit

Permalink
Fix missing control to force a filter update on pose updates (#248)
Browse files Browse the repository at this point in the history 
Major changes:
- Fixes lack of pose/transform update when setting a new initial pose.
- Add `request_nomotion_update` service (address #184)

Minor changes:
- Fix that ccache was not being used when building ROS 1.
- Rename `initialize_with_pose()` in `amcl_nodelet.xpp` to
`reinitialize_with_pose()`, which is the name of the same function in
`amcl_node.xpp`. The old name also overloaded the name of a different
function.

Signed-off-by: Gerardo Puga <[email protected]>
  • Loading branch information
@glpuga
glpuga authored Jul 22, 2023
1 parent fec06b7 commit ff0d4ed
Show file tree
Hide file tree
Showing 10 changed files with 290 additions and 33 deletions.
4 changes: 2 additions & 2 deletions beluga_amcl/README.md
View file
Original file line number Diff line number Diff line change
Expand Up @@ -61,6 +61,7 @@ Defaults are `map`, `odom` and `base`.
| Topic | Type | Description |
|------------------------------------|------------------|-------------------------------------------------------------------------------|
| `reinitialize_global_localization` | `std_srvs/Empty` | Request to reinitialize global localization without an initial pose estimate. |
| `request_nomotion_update` | `std_srvs/Empty` | Trigger a forced update of the filter estimates. |

## ROS 1 Interface

Expand Down Expand Up @@ -103,10 +104,9 @@ Defaults are `map`, `odom` and `base`.
| Topic | Type | Description |
|------------------------------------|-------------------|-------------------------------------------------------------------------------|
| `global_localization` | `std_srvs/Empty` | Request to reinitialize global localization without an initial pose estimate. |
| `request_nomotion_update` | `std_srvs/Empty` | Trigger a forced update of the filter estimates. |
| `set_map` | `nav_msgs/SetMap` | Set a new map and initial pose estimate. |

Note no `request_nomotion_update` service is available, as Beluga AMCL continually updates its estimate.

### Called Services

| Topic | Type | Description |
Expand Down
37 changes: 24 additions & 13 deletions beluga_amcl/include/beluga_amcl/filter_update_control/filter_update_control_mixin.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -45,23 +45,28 @@ class FilterUpdateControlInterface {
* @param laser_scan Laser-scan information, for the sensor update.
* @return true if the pose estimate can be expected to be different after this update.
*/
virtual bool update_filter(const motion_update_type& odom_to_base_transform, sensor_update_type&& laser_scan) = 0;
virtual bool update_filter(
const motion_update_type& odom_to_base_transform,
sensor_update_type&& laser_scan,
bool force_update) = 0;

/** \overload
* It allows specifying a sequenced execution policy.
*/
virtual bool update_filter(
std::execution::sequenced_policy exec_policy,
const motion_update_type& odom_to_base_transform,
sensor_update_type&& laser_scan) = 0;
sensor_update_type&& laser_scan,
bool force_update) = 0;

/** \overload
* It allows specifying a parallel execution policy.
*/
virtual bool update_filter(
std::execution::parallel_policy exec_policy,
const motion_update_type& odom_to_base_transform,
sensor_update_type&& laser_scan) = 0;
sensor_update_type&& laser_scan,
bool force_update) = 0;
};

/// Resampling policy poller.
Expand Down Expand Up @@ -102,11 +107,14 @@ class FilterUpdateControlMixin : public Mixin {
/**
* @param odom_to_base_transform Odom-to-base transform, for the motion update.
* @param laser_scan Laser-scan information, for the sensor update.
* @param force_update If true, the filter will be updated regardless of the resampling policies.
* @return true if the pose estimate can be expected to be different after this update.
*/
[[nodiscard]] bool update_filter(const motion_update_type& odom_to_base_transform, sensor_update_type&& laser_scan)
final {
return this->update_filter_impl(std::execution::sequenced_policy{}, odom_to_base_transform, std::move(laser_scan));
[[nodiscard]] bool update_filter(
const motion_update_type& odom_to_base_transform,
sensor_update_type&& laser_scan,
bool force_update) final {
return this->update_filter_impl(std::execution::seq, odom_to_base_transform, std::move(laser_scan), force_update);
}

/**
Expand All @@ -116,8 +124,9 @@ class FilterUpdateControlMixin : public Mixin {
[[nodiscard]] bool update_filter(
std::execution::sequenced_policy exec_policy,
const motion_update_type& odom_to_base_transform,
sensor_update_type&& laser_scan) final {
return this->update_filter_impl(exec_policy, odom_to_base_transform, std::move(laser_scan));
sensor_update_type&& laser_scan,
bool force_update) final {
return this->update_filter_impl(exec_policy, odom_to_base_transform, std::move(laser_scan), force_update);
}

/**
Expand All @@ -127,8 +136,9 @@ class FilterUpdateControlMixin : public Mixin {
[[nodiscard]] bool update_filter(
std::execution::parallel_policy exec_policy,
const motion_update_type& odom_to_base_transform,
sensor_update_type&& laser_scan) final {
return this->update_filter_impl(exec_policy, odom_to_base_transform, std::move(laser_scan));
sensor_update_type&& laser_scan,
bool force_update) final {
return this->update_filter_impl(exec_policy, odom_to_base_transform, std::move(laser_scan), force_update);
}

private:
Expand All @@ -140,15 +150,16 @@ class FilterUpdateControlMixin : public Mixin {
[[nodiscard]] bool update_filter_impl(
ExecutionPolicy exec_policy,
const motion_update_type& odom_to_base_transform,
sensor_update_type&& laser_scan) {
sensor_update_type&& laser_scan,
bool force_update) {
bool update_filter_estimate = false;

// based on NAV2 AMCL's decision tree to determine when to update, resample and
// update the filter estimates.

const auto far_enough_to_update = update_when_moving_policy_.do_filter_update(odom_to_base_transform);

if (far_enough_to_update) {
if (force_update || far_enough_to_update) {
this->self().update_motion(odom_to_base_transform);
this->self().sample(exec_policy);

Expand All @@ -170,7 +181,7 @@ class FilterUpdateControlMixin : public Mixin {
}
}

return update_filter_estimate;
return update_filter_estimate || force_update;
}
};

Expand Down
7 changes: 7 additions & 0 deletions beluga_amcl/include/beluga_amcl/private/amcl_node.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -62,6 +62,11 @@ class AmclNode : public rclcpp_lifecycle::LifecycleNode {
std::shared_ptr<rmw_request_id_t> request_header,
std::shared_ptr<std_srvs::srv::Empty::Request> request,
std::shared_ptr<std_srvs::srv::Empty::Response> response);
void nomotion_update_callback(
std::shared_ptr<rmw_request_id_t> request_header,
std::shared_ptr<std_srvs::srv::Empty::Request> req,
std::shared_ptr<std_srvs::srv::Empty::Response> res);

void reinitialize_with_pose(const Sophus::SE2d& pose, const Eigen::Matrix3d& covariance);

std::unique_ptr<LaserLocalizationInterface2d> particle_filter_;
Expand All @@ -75,6 +80,7 @@ class AmclNode : public rclcpp_lifecycle::LifecycleNode {
rclcpp::Subscription<geometry_msgs::msg::PoseWithCovarianceStamped>::SharedPtr initial_pose_sub_;
rclcpp::Subscription<nav_msgs::msg::OccupancyGrid>::SharedPtr map_sub_;
rclcpp::Service<std_srvs::srv::Empty>::SharedPtr global_localization_server_;
rclcpp::Service<std_srvs::srv::Empty>::SharedPtr nomotion_update_server_;

std::unique_ptr<tf2_ros::Buffer> tf_buffer_;
std::unique_ptr<tf2_ros::TransformBroadcaster> tf_broadcaster_;
Expand All @@ -88,6 +94,7 @@ class AmclNode : public rclcpp_lifecycle::LifecycleNode {
std::optional<Sophus::SE2d> latest_map_to_odom_transform_;

bool enable_tf_broadcast_{false};
bool force_filter_update_{false};
};

} // namespace beluga_amcl
Expand Down
7 changes: 6 additions & 1 deletion beluga_amcl/include/beluga_amcl/private/amcl_nodelet.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -76,9 +76,11 @@ class AmclNodelet : public nodelet::Nodelet {

bool global_localization_callback(std_srvs::Empty::Request& request, std_srvs::Empty::Response& response);

bool nomotion_update_callback(std_srvs::Empty::Request& request, std_srvs::Empty::Response& response);

void save_pose_timer_callback(const ros::TimerEvent& ev);

void initialize_with_pose(const Sophus::SE2d& pose, const Eigen::Matrix3d& covariance);
void reinitialize_with_pose(const Sophus::SE2d& pose, const Eigen::Matrix3d& covariance);

void update_covariance_diagnostics(diagnostic_updater::DiagnosticStatusWrapper& status);

Expand All @@ -96,6 +98,7 @@ class AmclNodelet : public nodelet::Nodelet {
ros::ServiceServer set_map_server_;
ros::ServiceClient get_map_client_;
ros::ServiceServer global_localization_server_;
ros::ServiceServer nomotion_update_server_;

bool configured_{false};
beluga_amcl::AmclConfig config_;
Expand All @@ -118,6 +121,8 @@ class AmclNodelet : public nodelet::Nodelet {

std::optional<std::pair<Sophus::SE2d, Eigen::Matrix3d>> last_known_estimate_;
std::optional<Sophus::SE2d> latest_map_to_odom_transform_;

bool force_filter_update_{false};
};

} // namespace beluga_amcl
Expand Down
31 changes: 28 additions & 3 deletions beluga_amcl/src/amcl_node.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -600,8 +600,17 @@ AmclNode::CallbackReturn AmclNode::on_activate(const rclcpp_lifecycle::State&) {
&AmclNode::global_localization_callback, this, std::placeholders::_1, std::placeholders::_2,
std::placeholders::_3),
rmw_qos_profile_services_default, common_callback_group);
#pragma GCC diagnostic pop
RCLCPP_INFO(get_logger(), "Created reinitialize_global_localization service");

nomotion_update_server_ = create_service<std_srvs::srv::Empty>(
"request_nomotion_update",
std::bind(
&AmclNode::nomotion_update_callback, this, std::placeholders::_1, std::placeholders::_2,
std::placeholders::_3),
rmw_qos_profile_services_default, common_callback_group);
RCLCPP_INFO(get_logger(), "Created request_nomotion_update service");

#pragma GCC diagnostic pop
}

return CallbackReturn::SUCCESS;
Expand Down Expand Up @@ -867,7 +876,9 @@ void AmclNode::laser_callback(ExecutionPolicy&& exec_policy, sensor_msgs::msg::L
utils::make_points_from_laser_scan(
*laser_scan, base_to_laser_transform, static_cast<std::size_t>(get_parameter("max_beams").as_int()),
static_cast<float>(get_parameter("laser_min_range").as_double()),
static_cast<float>(get_parameter("laser_max_range").as_double())));
static_cast<float>(get_parameter("laser_max_range").as_double())),
force_filter_update_);
force_filter_update_ = false;
const auto update_stop_time = std::chrono::high_resolution_clock::now();
const auto update_duration = update_stop_time - update_start_time;

Expand Down Expand Up @@ -899,7 +910,7 @@ void AmclNode::laser_callback(ExecutionPolicy&& exec_policy, sensor_msgs::msg::L
latest_map_to_odom_transform_ = pose * odom_to_base_transform.inverse();
}

// tranforms are always published to keep them current
// transforms are always published to keep them current
if (latest_map_to_odom_transform_ && enable_tf_broadcast_ && get_parameter("tf_broadcast").as_bool()) {
auto message = geometry_msgs::msg::TransformStamped{};
// Sending a transform that is valid into the future so that odom can be used.
Expand Down Expand Up @@ -940,6 +951,7 @@ void AmclNode::reinitialize_with_pose(const Sophus::SE2d& pose, const Eigen::Mat
try {
initialize_with_pose(pose, covariance, particle_filter_.get());
enable_tf_broadcast_ = true;
force_filter_update_ = true;
RCLCPP_INFO(
get_logger(),
"Particle filter initialized with %ld particles about "
Expand All @@ -962,6 +974,19 @@ void AmclNode::global_localization_callback(
particle_filter_->reinitialize();
RCLCPP_INFO(get_logger(), "Global initialization done!");
enable_tf_broadcast_ = true;
force_filter_update_ = true;
}

void AmclNode::nomotion_update_callback(
[[maybe_unused]] std::shared_ptr<rmw_request_id_t> request_header,
[[maybe_unused]] std::shared_ptr<std_srvs::srv::Empty::Request> req,
[[maybe_unused]] std::shared_ptr<std_srvs::srv::Empty::Response> res) {
if (!particle_filter_) {
RCLCPP_WARN(get_logger(), "Ignoring no-motion update request because the particle filter has not been initialized");
return;
}
RCLCPP_INFO(get_logger(), "Requesting no-motion update");
force_filter_update_ = true;
}

} // namespace beluga_amcl
Expand Down
38 changes: 30 additions & 8 deletions beluga_amcl/src/amcl_nodelet.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -104,6 +104,10 @@ void AmclNodelet::onInit() {
nh.advertiseService("global_localization", &AmclNodelet::global_localization_callback, this);
NODELET_INFO("Created global_localization service");

nomotion_update_server_ =
nh.advertiseService("request_nomotion_update", &AmclNodelet::nomotion_update_callback, this);
NODELET_INFO("Created request_nomotion_update service");

set_map_server_ = nh.advertiseService("set_map", &AmclNodelet::set_map_callback, this);
NODELET_INFO("Created set_map service");

Expand Down Expand Up @@ -161,7 +165,7 @@ void AmclNodelet::config_callback(beluga_amcl::AmclConfig& config, [[maybe_unuse
if (particle_filter_) {
const auto& [pose, covariance] = particle_filter_->estimate();
particle_filter_ = make_particle_filter(last_known_map_);
initialize_with_pose(pose, covariance);
reinitialize_with_pose(pose, covariance);
}
}

Expand Down Expand Up @@ -307,7 +311,7 @@ bool AmclNodelet::set_map_callback(nav_msgs::SetMap::Request& request, nav_msgs:
auto covariance = Eigen::Matrix3d{};
tf2::covarianceRowMajorToEigen(request.initial_pose.pose.covariance, covariance);

initialize_with_pose(pose, covariance);
reinitialize_with_pose(pose, covariance);

response.success = static_cast<unsigned char>(true);
return true;
Expand Down Expand Up @@ -345,10 +349,10 @@ void AmclNodelet::handle_map_with_default_initial_pose(const nav_msgs::Occupancy

if (should_reset_initial_pose && config_.set_initial_pose) {
const auto [pose, covariance] = extract_initial_pose(config_);
initialize_with_pose(pose, covariance);
reinitialize_with_pose(pose, covariance);
} else if (last_known_estimate_.has_value()) {
const auto& [pose, covariance] = last_known_estimate_.value();
initialize_with_pose(pose, covariance);
reinitialize_with_pose(pose, covariance);
}

last_known_map_ = map;
Expand Down Expand Up @@ -415,7 +419,9 @@ void AmclNodelet::laser_callback(ExecutionPolicy&& exec_policy, const sensor_msg
exec_policy, odom_to_base_transform,
utils::make_points_from_laser_scan(
*laser_scan, base_to_laser_transform, static_cast<std::size_t>(config_.laser_max_beams),
static_cast<float>(config_.laser_min_range), static_cast<float>(config_.laser_max_range)));
static_cast<float>(config_.laser_min_range), static_cast<float>(config_.laser_max_range)),
force_filter_update_);
force_filter_update_ = false;
const auto update_stop_time = std::chrono::high_resolution_clock::now();
const auto update_duration = update_stop_time - update_start_time;

Expand Down Expand Up @@ -483,7 +489,7 @@ void AmclNodelet::initial_pose_callback(const geometry_msgs::PoseWithCovarianceS
auto covariance = Eigen::Matrix3d{};
tf2::covarianceRowMajorToEigen(message->pose.covariance, covariance);

initialize_with_pose(pose, covariance);
reinitialize_with_pose(pose, covariance);
}

void AmclNodelet::save_pose_timer_callback(const ros::TimerEvent&) {
Expand All @@ -503,10 +509,11 @@ void AmclNodelet::save_pose_timer_callback(const ros::TimerEvent&) {
}
}

void AmclNodelet::initialize_with_pose(const Sophus::SE2d& pose, const Eigen::Matrix3d& covariance) {
void AmclNodelet::reinitialize_with_pose(const Sophus::SE2d& pose, const Eigen::Matrix3d& covariance) {
try {
beluga_amcl::initialize_with_pose(pose, covariance, particle_filter_.get());
initialize_with_pose(pose, covariance, particle_filter_.get());
enable_tf_broadcast_ = true;
force_filter_update_ = true;
NODELET_INFO(
"Particle filter initialized with %ld particles about "
"initial pose x=%g, y=%g, yaw=%g",
Expand All @@ -529,6 +536,21 @@ bool AmclNodelet::global_localization_callback(
particle_filter_->reinitialize();
NODELET_INFO("Global initialization done!");
enable_tf_broadcast_ = true;
force_filter_update_ = true;
return true;
}

bool AmclNodelet::nomotion_update_callback(
[[maybe_unused]] std_srvs::Empty::Request&,
[[maybe_unused]] std_srvs::Empty::Response&) {
std::lock_guard<std::mutex> lock(mutex_);
if (!particle_filter_) {
NODELET_WARN(
"Ignoring no-motion request because "
"the particle filter has not been initialized.");
return false;
}
force_filter_update_ = true;
return true;
}

Expand Down
Loading

0 comments on commit ff0d4ed

Please sign in to comment.