Memory grows overtime when calling service

[chanhhoang99]

Bug report

Required Info:

• Operating System:
  • Ubuntu 24.02
• Computer:
  • AMD box
• ROS2 Version:
  • Kilted
• Version or commit hash:
  • 895c63a
• DDS implementation:
  • CycloneDDS

Steps to reproduce issue

Calling isPoseOccupied in a tree xml config(https://github.com/ros-navigation/navigation2/blob/main/nav2_behavior_tree/plugins/condition/is_pose_occupied_condition.cpp)

Expected behavior

Memory consumption should not increase

Actual behavior

Memory consumption increase overtime

Reproduction instructions

Additional information

The background of this behavior is that I am trying to get a cost at a goal pose to check if it is occupied, if it is, I will stop/pause the robot until the obstacle move away. So the plugin is called in a time manner.(1hz)
With 1Hz the memory grow slowly and if I increase to 10 or 100Hz, this will take a lot of memory after a short time.
Below is the heap consumption trace

[chanhhoang99]

I think this may also relate to the rcl library.
There was a ticket that relates to rclpy that I filed in the past ros2/rclpy#1371 which also causing memory consumption to high when calling a service repeatedly.

[chanhhoang99]

Testing with another plugin IsPathValid but memory consumption did not increase overtime.

[mini-1235]

Hi @chanhhoang99, did you manually backport this to Kilted?

I am unable to reproduce the issue on rolling. Can you share the behavior tree you are using, and if I still can't reproduce the issue, I will test it on Kilted

[chanhhoang99]

Yes, I backported it on kilted.
Below is my tree

<?xml version="1.0" encoding="UTF-8"?>
<root BTCPP_format="4">
  <BehaviorTree ID="MainTree">
    <PauseResumeController pause_service_name="pause"
                           resume_service_name="resume">
      <RecoveryNode name="NavigateRecovery"
                    number_of_retries="10">
        <PipelineSequence name="NavigateWithReplanning">
          <ControllerSelector topic_name="controller_selector"
                              default_controller="FollowPath"
                              selected_controller="{selected_controller}"/>
          <PlannerSelector topic_name="planner_selector"
                           default_planner="GridBased"
                           selected_planner="{selected_planner}"/>
          <SpeedController min_rate="2.0"
                           max_rate="2.0"
                           min_speed="0.0"
                           max_speed="0.3">
            <RecoveryNode name="ComputePathToPose"
                          number_of_retries="1">
              <Sequence>
                <ComputePathToPose goal="{goal}"
                                   planner_id="{selected_planner}"
                                   path="{path}"
                                   error_code_id="{compute_path_error_code}"/>
                <SmoothPath smoother_id="simple_smoother"
                            unsmoothed_path="{path}"
                            check_for_collisions="false"
                            smoothed_path="{smooth_path}"
                            error_code_id="{smooth_path_error_code}"/>
              </Sequence>
              <ClearEntireCostmap name="ClearGlobalCostmap-Context"
                                  service_name="global_costmap/clear_entirely_global_costmap"/>
            </RecoveryNode>
          </SpeedController>
          <ReactiveSequence name="MonitorAndFollowPath">
            <RecoveryNode name="CheckGoalOccupied"
                          number_of_retries="30">
              <Inverter>
                <IsPoseOccupied pose="{goal}"
                                cost_threshold="200"
                                use_footprint="false"/>
              </Inverter>
              <SequenceWithMemory name="CancelingControlAndWait">
                <CancelControl name="ControlCancel"/>
                <ClearEntireCostmap name="ClearGlobalCostmap-Context"
                                    service_name="global_costmap/clear_entirely_global_costmap"/>
                <Wait wait_duration="1.0"/>
              </SequenceWithMemory>
            </RecoveryNode>
            <PathLongerOnApproach path="{path}"
                                  prox_len="1000000"
                                  length_factor="1.01">
              <RetryUntilSuccessful num_attempts="1">
                <SequenceWithMemory name="CancelingControlAndWait">
                  <CancelControl name="ControlCancel"/>
                  <Wait wait_duration="10.0"/>
                </SequenceWithMemory>
              </RetryUntilSuccessful>
            </PathLongerOnApproach>
            <RecoveryNode name="FollowPath"
                          number_of_retries="1">
              <FollowPath controller_id="{selected_controller}"
                          path="{smooth_path}"
                          error_code_id="{follow_path_error_code}"/>
              <ClearEntireCostmap name="ClearLocalCostmap-Context"
                                  service_name="local_costmap/clear_entirely_local_costmap"/>
            </RecoveryNode>
          </ReactiveSequence>
        </PipelineSequence>
        <Wait wait_duration="1.0"/>
      </RecoveryNode>
    </PauseResumeController>
  </BehaviorTree>

</root>

[chanhhoang99]

And I think you need to call navigateToPose many times to trigger the tree many times. I tested it in a loop to navigate around my warehouse.

[mini-1235]

And I think you need to call navigateToPose many times to trigger the tree many times. I tested it in a loop to navigate around my warehouse.

Could you clarify how you are doing that? Are you sending a new goal before the previous one is reached?

[chanhhoang99]

It is pretty much similar to https://github.com/ros-navigation/navigation2/blob/main/nav2_simple_commander/nav2_simple_commander/example_nav_to_pose.py

Need to complete 1 goal before attempt another in my case. But i think just repeatedly call navigateToPose also fine (no matter the goal is reached or not) just to trigger the tree.

[chanhhoang99]

This is my backporting work to kilted
https://github.com/huynhduc9905/navigation2/commits/v1.4.0-nav2-77db0a6/nav2_behavior_tree/plugins/condition/is_pose_occupied_condition.cpp

[SteveMacenski]

If this is related to lower layers, I think you should file a ticket with those respective repositories with a more basic unit test (i.e. some example node(s) that call a dummy service and show this growth behavior) so that maintainers can easily reproduce and that you can show clearly when the problem is resolved.

I don't think this is necessarily a ticket that should be tracked in Nav2 since its sufficiently below our levels of control / action. It may be worth trying with Fast-DDS as well quickly. If you stop seeing the issue, that's a good indication it might be either RMW Cyclone or Cyclone itself. If you don't stop seeing it, that doesn't definitely tell you much, but it could be at a higher level like rcl.

Feel free to try to reproduce @mini-1235 but @chanhhoang99 do you agree that this should be closed? I think this should be filed somewhere more ROS 2 core oriented (repo to depend on some debugging as to where you think it may be happening)

[chanhhoang99]

I don't confidently say the middleware is the root cause because when I replace the isPoseOccupied with isPathValid plugin(which also use service calling), memory seems not to grow overtime.

Do you guys think getting rid of calling a service in plugin like isPoseOccupied reasonable ? I mean we can make some change to the behavior tree to access the costmap and then access getCost method from there. Below is our work on that:
huynhduc9905@33fe668?fbclid=IwY2xjawOKKpRleHRuA2FlbQIxMABicmlkETFsbWJqWHB3SlBYcXJkeXdJc3J0YwZhcHBfaWQQMjIyMDM5MTc4ODIwMDg5MgABHpqVhMEvjCNiIuVUVtbko_KgmzIVBwDEgT3fEErbRAZfisUa5X2d_9-km8-M_aem_ZjVw5zuocuhHIApdLRkEjQ

The result is pretty same with the getCost srv, I tested calling the tree(navigateToPose) for 2 hours and the memory stay stable around ~80MB (starting at 76MB)

If we still call getCost service instead, that memory of bt_navigator node would grow significantly higher (around 300MB)
For now I am not sure if it is because of the middleware or not. But from my point of view, calling service with a very fast rate like that would be cumbersome.

[chanhhoang99]

Not calling service also helps reduce CPU usage

Below is without change

After change

1 more thing is that with the old way of calling getCost service, it some how increase the cpu usage of all other node on my machine(which actually does not interact with nav stack at all, my total cpu usage while navigating shrink from 70% to 40%)

[SteveMacenski]

I mean we can make some change to the behavior tree to access the costmap and then access getCost method from there

That has some non-negligible overhead computationally to pass around global costmaps. This is a major reason for the choice of the behavior server groupings. The service calls are far more efficient on the network and computation. These are huge messages which take non-linearly more compute time to process.

when I replace the isPoseOccupied with isPathValid plugin(which also use service calling), memory seems not to grow overtime.

That is interesting indeed. @mini-1235 do you reproduce? I believe this was a BT node that you added.

---

On your other metrics, it is worth breaking apart things into separate containers using use_composition:=False to see where changes are coming from. There's no reason to think that changing 1 service would drop your compute time by 30%. There are service calls all over the place in Nav2, unless these are somehow unique.

[chanhhoang99]

Interestingly, when I use use_composition:=False , CPU consumption is not that high compares to composable nodes. But the service called in tree plugin just continuously failed( which does not happen when I run as composable nodes)

One note is that the 30% consumption increase does not only comes from nav2 stack only, it also comes from our own nodes which does not interact with nav2 stack. For example there are nodes that just control the camera streaming normally run as 0.1% CPU usage. But when I run the nav2 stack and call navigatoPose to trigger the tree, that streaming node just increase CPU usage to 2% and same to all my nodes on the machine. These accumulate to total 30% different cpu usage. I feel like calling service getCost at high rate put more stress on the middleware.

So from my observation, below is the summarize:

1. Run nav2 normally(composable) with the tree above -> High CPU usage but none of service call failed
2. Run nav2 with use_composition:=False with the tree above -> CPU usage reduce significantly, service call failed randomly.

[mini-1235]

That is interesting indeed. @mini-1235 do you reproduce? I believe this was a BT node that you added.

I think I can reproduce the problem using the behavior tree @chanhhoang99 provided(on rolling + zenoh).

But if I switched to the default behavior tree, and modify the ComputePathToPose part(this might not be correct, just for testing):

<RateController hz="10.0">
          <RecoveryNode number_of_retries="1" name="ComputePathToPose">
            <Fallback>
              <IsPoseOccupied server_timeout="10" pose="{goal}"/>
              <ComputePathToPose goal="{goal}" path="{path}" planner_id="{selected_planner}" error_code_id="{compute_path_error_code}" error_msg="{compute_path_error_msg}"/>
            </Fallback>
            <Sequence>
                <WouldAPlannerRecoveryHelp error_code="{compute_path_error_code}"/>
                <ClearEntireCostmap name="ClearGlobalCostmap-Context" service_name="global_costmap/clear_entirely_global_costmap"/>
            </Sequence>
          </RecoveryNode>
        </RateController>

The memory stays relatively stable.

Also, I try to call the service with cli:

ros2 service call /local_costmap/get_cost_local_costmap nav2_msgs/srv/GetCosts "{                      6m 58s root@mini1235-XPS-15-9520  12:13:02
  use_footprint: false,
  poses: [
    {
      header: {frame_id: 'map'},
      pose: {
        position: {x: 1.0, y: 2.0, z: 0.0},
        orientation: {x: 0.0, y: 0.0, z: 0.0, w: 1.0}
      }
    }
  ]
}" -r 100

The memory remains stable as well.

I need to take a deeper look on the provided behavior tree to understand the issue