Adjust output time in costamp stack

[fmrico]

Hi,

This PR depends on EasyNavigation/EasyNavigation#85 and adjusts the processing output time to match its input time. I have just done this work in the costmap stack, but it should be done in all.

I'm opening this PR to start a discussion about time. IMHO, using "now()" for a timestamp is a mistake, because:

• Maps with updates based on perception with time t should be stamped with the same time t.
• Poses calculated by the localization algorithm should be stamped with the latest input information, usually the latest input perception time.
• Paths planned should be stamped with the latest time from their input (dynamic map, pose, or perceptions, if used).
• Output speed should be stamped with the latest time from its input (path, pose, or perceptions, if used).

The point is that it is not relevant the current time for stamping, but the input timestamps. What do you think? @juanscelyg @Butakus

[Copilot]

Pull request overview

This PR modifies the costmap stack to use input timestamps instead of now() for output messages, ensuring that processed data is stamped with the time of its source data rather than the current time. The changes aim to maintain temporal consistency across the navigation stack by propagating timestamps from perceptions, poses, and paths to derived outputs.

Key Changes:

• Introduced timestamp tracking mechanism through map_time in NavState to propagate perception timestamps
• Modified output messages (paths, maps, velocities, poses) to use the latest input timestamp instead of current time
• Replaced custom PI constant with standard M_PI in controller code

Reviewed changes

Copilot reviewed 6 out of 6 changed files in this pull request and generated 2 comments.

Show a summary per file

File	Description
CostmapPlanner.cpp	Computes latest timestamp from map, robot pose, and goals; stamps output path with this time
ObstacleFilter.cpp	Extracts latest timestamp from point perceptions and stores it in NavState as map_time
CostmapMapsManager.cpp	Propagates map_time from NavState to published occupancy grid messages
AMCLLocalizer.cpp	Tracks latest perception timestamp and uses it to stamp localization outputs
SerestController.cpp	Tracks latest input timestamps from path, odometry, and perceptions; stamps velocity commands accordingly
SerestController.hpp	Adds last_input_ts_ member variable to store latest input timestamp

---

💡 Add Copilot custom instructions for smarter, more guided reviews. Learn how to get started.

[Copilot]

The member variable last_input_ts_ is declared but never initialized. This will lead to undefined behavior when it's first accessed in fetch_required_inputs() at line 384 of the cpp file where it's used in a comparison operation. Initialize this variable in the constructor or in on_initialize() (similar to how last_update_ts_ is initialized at line 150 of the cpp file).

Suggested change

	rclcpp::Time last_input_ts_;
	rclcpp::Time last_input_ts_{};

[Copilot]

The member variable last_input_time_ is used here without being initialized first. This can occur if get_pose() is called from update_rt() (line 298) or update() (line 311) before any perception data or odometry messages have been received. This will result in undefined behavior. Initialize last_input_time_ in on_initialize() to a safe default value (e.g., using get_node()->now()).