Memory leak with CostmapToDynamicObstacles

[matteoderose]

Hi, I'm reporting a crash when using CostmapToDynamicObstales plugin for navigating a Turtlebot in a custom simulation environment.

Environment:

• Ubuntu 20.04
• ROS2 foxy
• Webots R2021a - for the simulation environment
• TurtleBot3 with LIDAR

I'm launching the Webots world in terminal1 and turtlebot3_navigation2 navigation2.launch.py in terminal 2. This works correctly.
Then, If I run the standalone_converter node with CostmapToDynamicObstales plugin from terminal 3, using gdb debug I get this:

matteodr@matteoNUC:~$ ros2 run --prefix 'gdb -ex run --args' costmap_converter standalone_converter --all-other-launch arguments
GNU gdb (Ubuntu 9.2-0ubuntu1~20.04) 9.2
Copyright (C) 2020 Free Software Foundation, Inc.
License GPLv3+: GNU GPL version 3 or later <http://gnu.org/licenses/gpl.html>
This is free software: you are free to change and redistribute it.
There is NO WARRANTY, to the extent permitted by law.
Type "show copying" and "show warranty" for details.
This GDB was configured as "x86_64-linux-gnu".
Type "show configuration" for configuration details.
For bug reporting instructions, please see:
--Type <RET> for more, q to quit, c to continue without paging--RET
<http://www.gnu.org/software/gdb/bugs/>.
Find the GDB manual and other documentation resources online at:
    <http://www.gnu.org/software/gdb/documentation/>.

For help, type "help".
Type "apropos word" to search for commands related to "word"...
Reading symbols from /home/matteodr/ros2_ws/install/costmap_converter/lib/costmap_converter/standalone_converter...
(No debugging symbols found in /home/matteodr/ros2_ws/install/costmap_converter/lib/costmap_converter/standalone_converter)
--Type <RET> for more, q to quit, c to continue without paging--c
Starting program: /home/matteodr/ros2_ws/install/costmap_converter/lib/costmap_converter/standalone_converter --all-other-launch arguments
[Thread debugging using libthread_db enabled]
Using host libthread_db library "/lib/x86_64-linux-gnu/libthread_db.so.1".
[New Thread 0x7ffff659d700 (LWP 11242)]
[New Thread 0x7ffff5d13700 (LWP 11243)]
[New Thread 0x7ffff5512700 (LWP 11244)]
[New Thread 0x7ffff4d11700 (LWP 11245)]
[New Thread 0x7ffff4510700 (LWP 11246)]
[New Thread 0x7ffff3d0f700 (LWP 11247)]
[New Thread 0x7ffff350e700 (LWP 11248)]
[New Thread 0x7ffff2d0d700 (LWP 11249)]
[New Thread 0x7ffff2383700 (LWP 11250)]
[New Thread 0x7ffff195e700 (LWP 11251)]
[New Thread 0x7ffff0cbc700 (LWP 11252)]
[INFO] [1625496476.614068004] [converter_costmap.converter_costmap]: 
	converter_costmap lifecycle node launched. 
	Waiting on external lifecycle transitions to activate
	See https://design.ros2.org/articles/node_lifecycle.html for more information.
[INFO] [1625496476.614976255] [converter_costmap.converter_costmap]: Creating Costmap
[New Thread 0x7fffd3fff700 (LWP 11253)]
[INFO] [1625496476.620708491] [converter_costmap.converter_costmap]: Configuring
[New Thread 0x7fffd37fe700 (LWP 11254)]
[INFO] [1625496476.631328644] [converter_costmap.converter_costmap]: Using plugin "static_layer"
[INFO] [1625496476.659515564] [converter_costmap.converter_costmap]: Subscribing to the map topic (/map) with transient local durability
[INFO] [1625496476.662064688] [converter_costmap.converter_costmap]: Initialized plugin "static_layer"
[INFO] [1625496476.662098273] [converter_costmap.converter_costmap]: Using plugin "obstacle_layer"
[INFO] [1625496476.663383857] [converter_costmap.converter_costmap]: Subscribed to Topics: 
[INFO] [1625496476.663492328] [converter_costmap.converter_costmap]: Initialized plugin "obstacle_layer"
[INFO] [1625496476.663522488] [converter_costmap.converter_costmap]: Using plugin "inflation_layer"
[INFO] [1625496476.664405979] [converter_costmap.converter_costmap]: Initialized plugin "inflation_layer"
[INFO] [1625496476.674781109] [converter_costmap.converter_costmap]: StaticLayer: Resizing costmap to 245 X 192 at 0.050000 m/pix
[INFO] [1625496476.680728679] [converter_costmap.converter_costmap]: Activating
[INFO] [1625496476.680761660] [converter_costmap.converter_costmap]: Checking transform
[INFO] [1625496476.680786888] [converter_costmap.converter_costmap]: Timed out waiting for transform from base_link to map to become available, tf error: Invalid frame ID "map" passed to canTransform argument target_frame - frame does not exist
[INFO] [1625496477.181059877] [converter_costmap.converter_costmap]: Timed out waiting for transform from base_link to map to become available, tf error: Lookup would require extrapolation into the past.  Requested time 148.592000 but the earliest data is at time 149.176000, when looking up transform from frame [base_link] to frame [map]
[INFO] [1625496477.680919632] [converter_costmap.converter_costmap]: Timed out waiting for transform from base_link to map to become available, tf error: Lookup would require extrapolation into the past.  Requested time 149.072000 but the earliest data is at time 149.176000, when looking up transform from frame [base_link] to frame [map]
[New Thread 0x7fffd2ffd700 (LWP 11255)]
[INFO] [1625496478.181705743] [converter_costmap.converter_costmap]: start
[INFO] [1625496478.726000752] [costmap_converter]: Standalone costmap converter: costmap_converter::CostmapToDynamicObstacles loaded.
[INFO] [1625496478.738832875] [costmap_converter.intra_node]: CostmapToDynamicObstacles: underlying costmap conversion plugin for static obstacles �uRVUU loaded.
[New Thread 0x7fffb7597700 (LWP 11256)]
[INFO] [1625496478.754237889] [costmap_converter.intra_node]: CostmapToDynamicObstacles: odom received.
[New Thread 0x7fffb5e08700 (LWP 11257)]
[New Thread 0x7fffb5607700 (LWP 11258)]
[New Thread 0x7fffa3fff700 (LWP 11259)]
malloc(): invalid size (unsorted)

Thread 16 "standalone_conv" received signal SIGABRT, Aborted.
[Switching to Thread 0x7fffb7597700 (LWP 11256)]
__GI_raise (sig=sig@entry=6) at ../sysdeps/unix/sysv/linux/raise.c:50
50	../sysdeps/unix/sysv/linux/raise.c: No such file or directory.
(gdb) backtrace
#0  __GI_raise (sig=sig@entry=6) at ../sysdeps/unix/sysv/linux/raise.c:50
#1  0x00007ffff7797859 in __GI_abort () at abort.c:79
#2  0x00007ffff78023ee in __libc_message (action=action@entry=do_abort, fmt=fmt@entry=0x7ffff792c285 "%s\n")
    at ../sysdeps/posix/libc_fatal.c:155
#3  0x00007ffff780a47c in malloc_printerr (str=str@entry=0x7ffff792ea50 "malloc(): invalid size (unsorted)") at malloc.c:5347
#4  0x00007ffff780d234 in _int_malloc (av=av@entry=0x7fffb0000020, bytes=bytes@entry=17856) at malloc.c:3736
#5  0x00007ffff780f419 in __GI___libc_malloc (bytes=17856) at malloc.c:3066
#6  0x00007ffff7a29b39 in operator new(unsigned long) () at /lib/x86_64-linux-gnu/libstdc++.so.6
#7  0x00007ffff01d24e5 in std::vector<std::vector<int, std::allocator<int> >, std::allocator<std::vector<int, std::allocator<int> > > >::_M_default_append(unsigned long) () at /home/matteodr/ros2_ws/install/costmap_converter/lib/libcostmap_converter.so
#8  0x00007ffff01cd7fc in costmap_converter::CostmapToPolygonsDBSMCCH::updateCostmap2D() ()
    at /home/matteodr/ros2_ws/install/costmap_converter/lib/libcostmap_converter.so
#9  0x00007ffff01e27f6 in costmap_converter::CostmapToDynamicObstacles::compute() ()
    at /home/matteodr/ros2_ws/install/costmap_converter/lib/libcostmap_converter.so
#10 0x00005555555734d7 in rclcpp::GenericTimer<std::_Bind<void (costmap_converter::BaseCostmapToPolygons::*(costmap_converter::BaseCostmapToPolygons*))()>, (void*)0>::execute_callback() ()
#11 0x00007ffff7ccf00d in rclcpp::Executor::execute_any_executable(rclcpp::AnyExecutable&) () at /opt/ros/foxy/lib/librclcpp.so
--Type <RET> for more, q to quit, c to continue without paging--

Everything works fine with CostmapToPolygonsDBSMCCH plugin instead