investigate memory corruption errors

[rayvburn]

Initially, this issue occurred only when visualizing the SFM force grid:

terminate called after throwing an instance of 'std::out_of_range'
  what():  vector::_M_range_check: __n (which is 4) >= this->size() (which is 0)

It was marked as backlog, as the force grid is not usually visualized.

However, after adding the selectRelevant method in the planner, the calculations on data shared with the ROS interface class started to take longer. At this point, additional errors occurred, e.g.:

double free or corruption (!prev)

terminate called after throwing an instance of 'std::out_of_range'
  what():  vector::_M_range_check: __n (which is 2) >= this->size() (which is 0)

free(): invalid next size (normal)

malloc(): memory corruption

usually at this line of the selectRelevant:

objects_sort.push_back({metric, obj});

Even simple:

std::vector<Person> people = *people_;

in the planner's createEnvironmentModel sometimes threw an exception of uninitialized_copy of a std::vector.

[rayvburn]

After correcting the concurrency issues, another problem has arisen, namely delays in observation buffer:

[ INFO] [1695212573.242417939, 21.616000000]: Full control cycle time: real {0.691746814}, sim {0.303000000}
[ WARN] [1695212573.242788599, 21.616000000]: The scan_narrow observation buffer has not been updated for 0.35 seconds, and it should be updated every 0.30 seconds.
[ WARN] [1695212573.513663161, 21.760000000]: [/move_base]:Sensor data is out of date, we're not going to allow commanding of the base for safety
[ INFO] [1695212573.513761709, 21.760000000]: Full control cycle time: real {0.000195441}, sim {0.000000000}

and the following loop execution does nothing (takes 0 ms as shown above).

The following code of the move_base is responsible for not updating the costmap during the call to trajectory planner's computeVelocityCommands: https://github.com/ros-planning/navigation/blob/9ad644198e132d0e950579a3bc72c29da46e60b0/move_base/src/move_base.cpp#L910

EDIT: Same happens with DWA. Couldn't observe in TEB (even at computation times at 500 ms).