diff --git a/crosbot_explore/include/crosbot_explore/explorerROS.hpp b/crosbot_explore/include/crosbot_explore/explorerROS.hpp index 948bdfc3ac52e8a1f9313afb71b7ef8113f5f98b..5cf3583957cdc680af45f1bff15298286ae95415 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 77a895e8f964bc2c4fec75ecae8fcd2a5f51851e..49d27c12a1324e06eb87dd505d479698d560c64d 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); }