removed the deep copy for the costmap_raw_ data using memcpy in prepareCostmap under costmap_2d_publisher.cpp

[glitchhopcore]

---

Basic Info

Info
Ticket(s) this addresses	#4914
Primary OS tested on	Ubuntu
Robotic platform tested on	gazebo simulation
Does this PR contain AI generated software?	No

---

Description of contribution in a few bullet points

• I modified the loop that was being used to copy raw costmap by using a memcpy instead

Description of documentation updates required from your changes

• None would be required as it is just an enhancement

---

Future work that may be required in bullet points

• Further profiling costmap related functionalities to identify which processes are costing us the most.

For Maintainers:

• Check that any new parameters added are updated in docs.nav2.org
• Check that any significant change is added to the migration guide
• Check that any new features OR changes to existing behaviors are reflected in the tuning guide
• Check that any new functions have Doxygen added
• Check that any new features have test coverage
• Check that any new plugins is added to the plugins page
• If BT Node, Additionally: add to BT's XML index of nodes for groot, BT package's readme table, and BT library lists

[codecov]

Codecov Report

All modified and coverable lines are covered by tests ✅

Files with missing lines	Coverage Δ
nav2_costmap_2d/src/costmap_2d_publisher.cpp	61.53% <ø> (-0.54%)	⬇️

... and 3 files with indirect coverage changes

[SteveMacenski]

Per our email, please get some metrics. You can create a simple program that creates the Costmap2DPublisher object, gives it a costmap of realistic size and call prepareCostmap on it in a loop 10000x. Before and after the loop, use Chrono to take the current time and print to screen how long it took to do that operation. Change to using memcpy and then run it again to see the performance change.

For the local costmap, a realistic size might be 10x10m at 0.05m resolution. For a global, maybe a 100x100m at 0.05m resolution.

[glitchhopcore]

Thanks for the review @SteveMacenski, I will try to do so and share some results here before and after using memcpy!

[glitchhopcore]

I have added some results from benchmarking the prepareCostmap() method over 10,000 iterations and its performance improvements before and after using memcpy.

For a local costmap of dimensions 10x10m:

Original method: ~287ms

After memcpy implementation: ~24ms

For a global costmap of dimensions 100x100m:

Original method: ~33,888ms

After memcpy implementation: ~5,607ms

I used the following code for benchmarking these results:

#include <chrono>
#include <iostream>
#include <random>
#include "rclcpp/rclcpp.hpp"
#include "rclcpp_lifecycle/lifecycle_node.hpp"
#include "nav2_costmap_2d/costmap_2d_publisher.hpp"
#include "nav2_costmap_2d/costmap_2d.hpp"

int main() {
    rclcpp::init(0, nullptr);
    auto node = std::make_shared<rclcpp_lifecycle::LifecycleNode>("costmap_bench");
    node->configure();
    node->activate();

    nav2_costmap_2d::Costmap2D cmap(2000, 2000, 0.05, 0.0, 0.0); // or 200, 200, 0.05 for local costmap
    
    std::mt19937 gen(std::random_device{}());
    std::uniform_int_distribution<> dist(0, 255);
    
    for(unsigned int i = 0; i < cmap.getSizeInCellsX(); i++) {
        for(unsigned int j = 0; j < cmap.getSizeInCellsY(); j++) {
            cmap.setCost(i, j, dist(gen));
        }
    }

    nav2_costmap_2d::Costmap2DPublisher pub(node, &cmap, "map", "costmap", true, 0.0);

    auto t1 = std::chrono::high_resolution_clock::now();
    for(int i = 0; i < 10000; i++) {
        pub.prepareCostmap();
    }
    auto t2 = std::chrono::high_resolution_clock::now();
    
    std::cout << "Time taken with memcpy: " << std::chrono::duration_cast<std::chrono::milliseconds>(t2 - t1).count() << "ms\n";

    node->deactivate();
    node->cleanup();
    node->shutdown();
    rclcpp::spin_some(node->get_node_base_interface());
    rclcpp::shutdown();
    return 0;
}

[SteveMacenski]

Ah very nice and perfect, a good 5-10x speed up!

[SteveMacenski]

@glitchhopcore I'm curious: anywhere else in this class we can optimize performance? These are the single largest data structures nav2 publishes internally, so improvements in the Costmap2DPublisher and Costmap2DSubscriber are significant.

Also, might want to look at the subscriber too for how we copy data around 😉 https://github.com/ros-navigation/navigation2/blob/main/nav2_costmap_2d/src/costmap_subscriber.cpp