From 81500a9e2ae42cb9e19a54af9598a1a4a7bbe1ff Mon Sep 17 00:00:00 2001 From: Timothy Wiley Date: Sat, 11 Mar 2017 12:27:46 +1100 Subject: [PATCH] crosbot_explore: Actually use the mutex on loading new maps --- .../include/crosbot_explore/explorerROS.hpp | 2 -- crosbot_explore/src/explorerROS.cpp | 13 ++++++++++++- 2 files changed, 12 insertions(+), 3 deletions(-) diff --git a/crosbot_explore/include/crosbot_explore/explorerROS.hpp b/crosbot_explore/include/crosbot_explore/explorerROS.hpp index 948bdfc..5cf3583 100644 --- a/crosbot_explore/include/crosbot_explore/explorerROS.hpp +++ b/crosbot_explore/include/crosbot_explore/explorerROS.hpp @@ -56,8 +56,6 @@ protected: ReadWriteLock rosLock; nav_msgs::OccupancyGridConstPtr latestMap; nav_msgs::PathConstPtr latestHistory; - - ReadWriteLock voronoiLock; VoronoiGridPtr latestVoronoi; public: diff --git a/crosbot_explore/src/explorerROS.cpp b/crosbot_explore/src/explorerROS.cpp index 77a895e..49d27c1 100644 --- a/crosbot_explore/src/explorerROS.cpp +++ b/crosbot_explore/src/explorerROS.cpp @@ -301,7 +301,12 @@ void ExplorerROSNode::subscribeGrid(ros::NodeHandle& nh) { } VoronoiGridPtr ExplorerROSNode::getLatestVoronoi() { - nav_msgs::OccupancyGridConstPtr latestMap = this->latestMap; + nav_msgs::OccupancyGridConstPtr latestMap; + { + Lock lock(rosLock, false); + latestMap = this->latestMap; + } + if (latestMap == NULL) { return NULL; } @@ -333,6 +338,12 @@ Pose ExplorerROSNode::getLatestPose(std::string frame_id, Time timestamp) { } Pose ExplorerROSNode::getLatestPose() { + nav_msgs::OccupancyGridConstPtr latestMap; + { + Lock lock(rosLock, false); + latestMap = this->latestMap; + } + if (latestMap == NULL) { return Pose(INFINITY, INFINITY, INFINITY); } -- GitLab