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

Added mutex to prevent SEGFAULT on map change in AMCL #2933

Merged
merged 2 commits into from
May 5, 2022
Merged

Added mutex to prevent SEGFAULT on map change in AMCL #2933

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
0140969
Added mutex to prevent SEGFAULT on map change
May 4, 2022
4ebee23
Removing redundant mutex
May 5, 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
3 changes: 1 addition & 2 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 @@ -132,7 +132,7 @@ class AmclNode : public nav2_util::LifecycleNode
bool first_map_only_{true};
std::atomic<bool> first_map_received_{false};
amcl_hyp_t * initial_pose_hyp_;
std::recursive_mutex configuration_mutex_;
std::recursive_mutex mutex_;
rclcpp::Subscription<nav_msgs::msg::OccupancyGrid>::ConstSharedPtr map_sub_;
#if NEW_UNIFORM_SAMPLING
static std::vector<std::pair<int, int>> free_space_indices;
Expand Down Expand Up @@ -238,7 +238,6 @@ class AmclNode : public nav2_util::LifecycleNode
*/
static pf_vector_t uniformPoseGenerator(void * arg);
pf_t * pf_{nullptr};
std::mutex pf_mutex_;
bool pf_init_;
pf_vector_t pf_odom_pose_;
int resample_count_{0};
Expand Down
9 changes: 5 additions & 4 deletions nav2_amcl/src/amcl_node.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -465,7 +465,7 @@ AmclNode::globalLocalizationCallback(
const std::shared_ptr<std_srvs::srv::Empty::Request>/*req*/,
std::shared_ptr<std_srvs::srv::Empty::Response>/*res*/)
{
std::lock_guard<std::mutex> lock(pf_mutex_);
std::lock_guard<std::recursive_mutex> cfl(mutex_);

RCLCPP_INFO(get_logger(), "Initializing with uniform distribution");

Expand All @@ -491,7 +491,7 @@ AmclNode::nomotionUpdateCallback(
void
AmclNode::initialPoseReceived(geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr msg)
{
std::lock_guard<std::mutex> lock(pf_mutex_);
std::lock_guard<std::recursive_mutex> cfl(mutex_);

RCLCPP_INFO(get_logger(), "initialPoseReceived");

Expand Down Expand Up @@ -526,6 +526,7 @@ AmclNode::initialPoseReceived(geometry_msgs::msg::PoseWithCovarianceStamped::Sha
void
AmclNode::handleInitialPose(geometry_msgs::msg::PoseWithCovarianceStamped & msg)
{
std::lock_guard<std::recursive_mutex> cfl(mutex_);
// In case the client sent us a pose estimate in the past, integrate the
// intervening odometric change.
geometry_msgs::msg::TransformStamped tx_odom;
Expand Down Expand Up @@ -590,7 +591,7 @@ AmclNode::handleInitialPose(geometry_msgs::msg::PoseWithCovarianceStamped & msg)
void
AmclNode::laserReceived(sensor_msgs::msg::LaserScan::ConstSharedPtr laser_scan)
{
std::lock_guard<std::mutex> lock(pf_mutex_);
std::lock_guard<std::recursive_mutex> cfl(mutex_);

// Since the sensor data is continually being published by the simulator or robot,
// we don't want our callbacks to fire until we're in the active state
Expand Down Expand Up @@ -1127,7 +1128,7 @@ AmclNode::mapReceived(const nav_msgs::msg::OccupancyGrid::SharedPtr msg)
void
AmclNode::handleMapMessage(const nav_msgs::msg::OccupancyGrid & msg)
{
std::lock_guard<std::recursive_mutex> cfl(configuration_mutex_);
std::lock_guard<std::recursive_mutex> cfl(mutex_);

RCLCPP_INFO(
get_logger(), "Received a %d X %d map @ %.3f m/pix",
Expand Down