Mutex and time efficiency in callbacks

edit

Hi, after some experience with ROS I cannot decide how to implement callbacks and algorithms where I need the best possible performance. For example, let us image we have a octomap publisher. My node subscribes to the octomap publisher. The octomap information is used for collision checking during path planning (common approach).

The problem I find is when to find between the message itself and the current octree. During planning, the octree has to be constant, or segmentation faults will appear since the pointers has changed. My previous approach was something like:

class Planner {
      ...
     void collision_map_callback(const octomap_msgs::Octomap &map_msg) {
         _map_msg = map_msg;
      }

      void Plan {
           octomap::Octree *_octree = dynamic_cast<octomap::OcTree*>(octomap_msgs::msgToMap(map_msg));
      }
      .....
 };

However, the Plan method has to "waste" time in the real octree computation (as I need to use the octree iterators). Therefore, I thought to do the conversion in the callback, but that is against callbacks philosophy, and mutexes have to be included, as Plan() can be being executed during a callback execution. This is my current approach as the Plan() member is what I really care about (I have the same cases in other classes, where the octree is created much more frequently).

Finally, I thought in another solution: the callback only copies the message and activates a flag in the class to update _octree. Use a second _octree2 pointer in a separated thread to create the octree from the last message. And finally, when the class is not using the _octree (between iterations of a local planner for instance), change pointers between _octree and _octree2. This way, _octree is always updated, the callback has no expensive computations, and there is no need for threads.

My point is that this last solution requires twice as memory (for short periods of time) and is a bit "complex". I imagine as there are a lot of people using ROS, this problem has been already faced and I would like to hear some feedback about my different solutions or any better solution.

Thanks!