Ensure latched messages are updated on every split

[ggorjup]

Summary

This fixes an issue with #1850 - latched messages from the same publisher are not updated in the Recorder.

Steps to Reproduce

1. Create and run a node that periodically publishes latched messages, for example:

#!/usr/bin/env python3
import rospy
from std_msgs.msg import String

pub = rospy.Publisher('chatter', String, latch=True)
rospy.init_node('talker')
for i in range(20):
    pub.publish(str(i))
    rospy.sleep(5)

2. Record the latched topic in splits:
   rosrun rosbag record -a --repeat-latched --duration 10 --split
3. Check messages on the target topic in the last split:
   rostopic echo -b <last_split>.bag chatter

Expected behavior

The first message in the last split is the value of the latched topic when the split was started (e.g., data: "18").

Actual behavior

The first message in the last split is the value of the latched topic when the rosbag record was run (e.g., data: "0").

Implications

This behavior causes issues when recording latched topics that are not often updated. If the rosbag record is a long-running process, the latched topic values in some splits may be several hours old (if the publisher does not send an update within the split time frame).

Cause

Latched messages are stored as a std::map<std::pair<std::string, std::string>, OutgoingMessage>, where the key is a pair of strings (topic and publisher name) and the value is the latched message.

When updating the latched_msgs_ map, the std::map::insert() function is used. This behaves as expected when the key does not yet exist in the map. However, if the key exists, the old value is not updated:

Because element keys in a map are unique, the insertion operation checks whether each inserted element has a key equivalent to the one of an element already in the container, and if so, the element is not inserted, returning an iterator to this existing element

-- std::map::insert

This effectively means that the first value (latched message) that was inserted into the map will persist across the whole rosbag record.

Solution

Check for insertion success and assign value manually if required (see commit).

[ghost]

I am running into issues caused by this, which strongly limits the viability of --repeat-latched.

@mjcarroll
Would it be possible to have a look at merging this bugfix?

[AndreasNagel]

Suggested a change, but even without it, merging what is here is better than keeping a broken implementation around.

[AndreasNagel]

Suggested change

	auto const result = latched_msgs_.insert({{subscriber->getTopic(), it2->second}, out});
	latched_msgs_[{subscriber->getTopic(), it2->second}] = out;

[AndreasNagel]

The [] operator does exactly what's expected here, in case the key doesn't exist in the map, it will be created and when it does exist, it will be updated.

[ggorjup]

Thanks for the suggestion - the [] operator is indeed cleaner since it removes the need for the extra manual check (in lines 351-354).

I'll try to update it shortly (I don't have the test environment handy anymore).