Change publishers to publish unique ptrs

[Sarath18]

---

Basic Info

Info	Please fill out this column
Ticket(s) this addresses	#1672
Primary OS tested on	Ubuntu 18.04
Robotic platform tested on	gazebo simulation of TurtleBot3 (Waffle)

---

Description of contribution in a few bullet points

• Change all the publishers to publish unique pointers.

Description of documentation updates required from your changes

• Publishers in navigation2 should publish unique pointers rather than raw messages.

---

Future work that may be required in bullet points

• I have tried to cover all ROS publishers in navigation2. I'll make the necessary changes if any required.

[fmrico]

isnt't it initial_pose_pub_->publish(std::move(*testPose_))?

[SteveMacenski]

Not quite, you're moving the pointer ownership, not the object.

[fmrico]

same here initial_pose_pub_->publish(std::move(*testPose_))?

[SteveMacenski]

Not quite, you're moving the pointer ownership, not the object.

[fmrico]

same here initial_pose_pub_->publish(std::move(*testPose_))?

[SteveMacenski]

Not quite, you're moving the pointer ownership, not the object.

[Sarath18]

I realized a few mistakes on my side. Firstly, I shouldn't have modified the code and the test cases together.

Secondly, modifications to this test case essentially do the same thing. During publishing, it is dereferencing the pointer which leads to the publishing of raw message instead.

isnt't it initial_pose_pub_->publish(std::move(*testPose_))?

@fmrico, let's look at the changes you suggested together. testPose_ is a unique_ptr inside the class and modifications to testPose_ happens in multiple functions of the class. So when the publish statement is called, the pointer gets dereferenced and then the data is moved to the publisher. Now the block that contained the data is deallocated and the pointer is still valid. This resulted in a failure of the test case. Even initial_pose_pub_->publish(std::move(testPose_)); won't work as this statement will pass the ownership to the publisher and testPose_ will become a null pointer.

I am unclear on how to tackle this problem. If testPose_ was a normal data member instead of a pointer, how do publish a unique pointer when publish statement is called?
The current implementation was to create a unique_ptr using the raw message and then publish it. But this might problems again as it will return a pointer to a newly allocated memory after moving the contents from the variable. This will deallocate that data member. Looking at it again I found out that this implementation worked out because data is being populated every time before being published.
Please do correct me if I am wrong.

[fmrico]

Hi @Sarath18,

You are right!! But in this case, I suggest not to change this file. As you said, this is test code and maybe it shouldn't be changed.

I have run your code in sim and seems to be working properly. IMHO you did great work!!

I have done a little experiment comparing your improved code and the baseline. The robot performs a navigation along three waypoint, as shown in the desktop capture. The files contain a record every second of cpu (2nd column) and mem (3rd column).

Hope it helps!!
cpu_baseline.txt
cpu_improve.txt

[Sarath18]

You are right!! But in this case, I suggest not to change this file. As you said, this is test code and maybe it shouldn't be changed.

I should revert back the test cases.

I have run your code in sim and seems to be working properly. IMHO you did great work!!

Great! Thanks :)

I have done a little experiment comparing your improved code and the baseline. The robot performs a navigation along three waypoint, as shown in the desktop capture. The files contain a record every second of cpu (2nd column) and mem (3rd column).

Thank you for benchmarking the updates. Can you please clarify what baseline is? Is without the updates? Looking at the results you provided, I noticed that in general cpu_improve.txt shows more cpu usage compared to the baseline while memory usage remains the same. Weird.

[fmrico]

Hi @Sarath18

Maybe we should repeat this experiment. I think that the first values of improved (this PR) are affected in some way by the execution of the experiment with baseline (master).

Anyway, If you see the sequence discarding first results, using unique_ptr significative reduces the cpu computation (second column). There are no significant changes in the mem computation (3rd column).

If you want to obtain your own values, in the screenshot you can find the command that I used to obtain these results.

[Sarath18]

Hi @fmrico,

Anyway, If you see the sequence discarding first results, using unique_ptr significative reduces the cpu computation (second column). There are no significant changes in the mem computation (3rd column).

Thank you for clarifying. I made a plot of your results and that helped me understand the CPU utilization difference.

The current implementation was to create a unique_ptr using the raw message and then publish it. But this might problems again as it will return a pointer to a newly allocated memory after moving the contents from the variable. This will deallocate that data member. Looking at it again I found out that this implementation worked out because data is being populated every time before being published.

Any ideas on how to solve Costmap and Grid publishers in costmap_2d_publisher.cpp? @SteveMacenski mentioned in the ticket:

We should transition all ROS publishers in navigation (and anywhere we can) to this method. This is especially important for topics publishing larger messages like pointclouds, maps, and images.

[SteveMacenski]

I see 2 publishes missing in the map server for the map and in the planner server publishing paths. Otherwise LGTM

[SteveMacenski]

@Sarath18 when you do std::move it nulls out the pointer, so it needs to be reallocated. For all the cases when you publish something, you need to make sure that the variable is local to the function so that a new unique_ptr will be made each time its called or that the class variables are reallocated before a next call.

i'm not overly concerned with changing the tests because those aren't used in production and this is a production optimization.

Those charts do look suspect. I don't see why gazebo would drop so much CPU based on a far downstream change. I think this would have to be re-run at least 5 times on the same computer to get results. I would be really interested in this analysis though (and a number like saved N%), that could make a very useful write up in the ROS2 documentation and if we had tangible proof of its impact, that could be really valuable.

See below on costmap_2d_publisher

@fmrico what script did you use for that? Can you share it with us? Might be useful to add to the tools/ directory. If we can get the recording code and then a plotting code, that could be useful for making optimizations.

[Sarath18]

For all the cases when you publish something, you need to make sure that the variable is local to the function

Got it.

i'm not overly concerned with changing the tests because those aren't used in production and this is a production optimization.

I reverted back the test case anyway.

Those charts do look suspect. I don't see why gazebo would drop so much CPU based on a far downstream change. I think this would have to be re-run at least 5 times on the same computer to get results.

I'll rerun the tests as you said on my computer and then share the results. @fmrico, it would be great if you could throw some more light on this.

@fmrico what script did you use for that? Can you share it with us?

You can find the script in the screenshot @fmrico attached above.

while true; do ps -a -o comm= -o pcpu= -o pmem=; echo "================================"; sleep 1; done >> cpu_baseline.txt

I would be really interested in this analysis though (and a number like saved N%), that could make a very useful write up in the ROS2 documentation and if we had tangible proof of its impact, that could be really valuable.

Might be useful to add to the tools/ directory. If we can get the recording code and then a plotting code, that could be useful for making optimizations.

I believe we both have written rough scripts to test this feature. If you really want to add it to the tools directory we can polish the logging/visualization before that. I think this can be moved to a separate ticket to populate ideas and incorporate more features.

[fmrico]

Hi @SteveMacenski

As @Sarath18 wrote, the command used was very simple:

while true; do ps -a -o comm= -o pcpu= -o pmem=; echo "================================"; sleep 1; done >> cpu_baseline.txt

It's simple, but it gives us a quick idea about the improvement

Francisco

[Sarath18]

I see 2 publishes missing in the map server for the map and in the planner server publishing paths.

Hey @SteveMacenski, I didn't update these publishers to publish unique_ptrs because they were not in the local scope of the publisher and were being used in multiple places. I see a few ways to update this but I am not sure if that is the best way to do so. result in planner_server is a shared pointer being passed to action_server_ and msg_ (occupancy_grid) in map_server is a unique pointer that is being updated in different methods of the class.

I have made all other requested changes.

[SteveMacenski]

I think the map server msg_ is only published by 1 entity (correct me if I'm wrong). You should be able to create the unique_ptr on bringup, and then after std::move to publish, instantiate it again. If everything is single threaded, then there's no risk of someone calling on that nullptr.

For the planner the path isn't a shared pointer, the result is a shared pointer. You can change the path return from the planner to be a unique_ptr local variable that's dereferenced and copied to the result shared pointer. There's going to be 1 copy somewhere, lets just own it in the code explicitly.

[SteveMacenski]

You can leave grid_ as a class variable, in fact, I'd prefer it. You just need to std::make_unique<nav_msgs::msg::OccupancyGrid>(); on the class variable at the start of prepare grid to instantiate it. I'd revert this commit and just make the simpler change here and in publish grid. You don't want to return smart pointer types.

[SteveMacenski]

Looks like you have some merge conflicts (somehow?) with amcl

[Sarath18]

For the planner the path isn't a shared pointer, the result is a shared pointer. You can change the path return from the planner to be a unique_ptr local variable that's dereferenced and copied to the result shared pointer. There's going to be 1 copy somewhere, lets just own it in the code explicitly.

If there is going to be one copy somewhere, why not do it just before the publish (just before or inside publishPlan)? By doing this we can incorporate any updates to the path (if any) before publishing. I tested it out and it works just fine.

I think the map server msg_ is only published by 1 entity (correct me if I'm wrong).

There are two publishers in the MapServerClass. One int the on_activate lifecycle state and the other one in the loadMapCallback.

You should be able to create the unique_ptr on bringup, and then after std::move to publish, instantiate it again. If everything is single threaded, then there's no risk of someone calling on that nullptr.

I did this. But I had to populate the OccupancyGrid by calling loadMapFromYaml after instantiation. I don't think this is a good way to do that. By having another class variable that stores the OccupancyGrid and then copying it into unique_ptr we will again face a 1 copy somewhere.

Looks like you have some merge conflicts (somehow?) with amcl

This is because of the update to amcl particle cloud to incorporate weights. I'll merge this once everything else is solved.

I have made all other requested changes.

[SteveMacenski]

If there is going to be one copy somewhere, why not do it just before the publish (just before or inside publishPlan)? By doing this we can incorporate any updates to the path (if any) before publishing. I tested it out and it works just fine.

You could also do make_unique<...>(result->path) as well, sure. I was just pointing out that the path wasn't the shared ptr, it was a member of one so there's no a conversion from shared_ptr -> unique_ptr required. Its just copying the contents of a memory of the result to the unique_ptr to publish.

There are two publishers in the MapServerClass. One int the on_activate lifecycle state and the other one in the loadMapCallback.

That's fine. In both places do

occ_pub_->publish(std::move(msg_));
msg_ = ... make_unique<>();

The only issue you would run into is if the activate was called at the exact same time as loadMapCallback, which I don't think is possible since the on_activate is called from a callback that will be taking up the node executor so no other service calls can be made, and vise versa.

But I had to populate the OccupancyGrid by calling loadMapFromYaml after instantiation.

You are correct. Mhm. That makes map server really interesting. Maybe we should change the map msg_ to be a stack variable and we copy it to a unique_ptr to publish.

[Sarath18]

@SteveMacenski, I have made all the changes. If everything seems fine, I'll merge master here and fix any conflicts that occur.

I am not sure if I encounter a bug or not but ament_cpplint was not configured for nav2_costmap_2d when running tests. I had to manually run the checks and resolve the issues in that package.

[SteveMacenski]

One last question and LGTM

@AlexeyMerzlyakov can you review the map server part?

[SteveMacenski]

I am not sure if I encounter a bug or not but ament_cpplint was not configured for nav2_costmap_2d when running tests. I had to manually run the checks and resolve the issues in that package.

You're welcome to add that then.

Also, you still have conflicts to resolve.

[codecov]

Codecov Report

Merging #1673 into master will decrease coverage by 0.28%.
The diff coverage is 15.96%.

@@            Coverage Diff             @@
##           master    #1673      +/-   ##
==========================================
- Coverage   39.41%   39.13%   -0.29%     
==========================================
  Files         205      205              
  Lines       10302    10310       +8     
  Branches     4187     4204      +17     
==========================================
- Hits         4061     4035      -26     
- Misses       3665     3693      +28     
- Partials     2576     2582       +6     

Flag	Coverage Δ
#project	39.13% <15.96%> (-0.29%)	⬇️

Impacted Files	Coverage Δ
nav2_bt_navigator/src/ros_topic_logger.cpp	38.09% <0.00%> (-4.77%)	⬇️
nav2_controller/src/nav2_controller.cpp	25.65% <0.00%> (-0.53%)	⬇️
...d/include/nav2_costmap_2d/costmap_2d_publisher.hpp	100.00% <ø> (ø)
...costmap_2d/include/nav2_costmap_2d/voxel_layer.hpp	8.00% <ø> (ø)
nav2_costmap_2d/src/costmap_2d_cloud.cpp	0.00% <0.00%> (ø)
nav2_costmap_2d/src/costmap_2d_markers.cpp	0.00% <0.00%> (ø)
nav2_costmap_2d/src/costmap_2d_ros.cpp	27.23% <0.00%> (ø)
nav2_dwb_controller/dwb_core/src/publisher.cpp	21.55% <0.00%> (-0.27%)	⬇️
...lifecycle_manager/src/lifecycle_manager_client.cpp	0.00% <0.00%> (ø)
nav2_planner/src/planner_server.cpp	24.60% <0.00%> (-1.00%)	⬇️
... and 15 more

---

Continue to review full report at Codecov.

Legend - Click here to learn more
Δ = absolute <relative> (impact), ø = not affected, ? = missing data
Powered by Codecov. Last update 0c4aea8...e3e7314. Read the comment docs.

[AlexeyMerzlyakov]

@AlexeyMerzlyakov can you review the map server part?

Checked. Map server change looks good to me. I agree that there is no need to keep msg_ to be unique_ptr since it will be converted to rvalue later.

[SteveMacenski]

Thanks for this work, you've probably touched the most code throughout the stack of the new developers! Learn anything from looking at these files?

[Sarath18]

Thank you for this opportunity. This helped me to understand the navigation2 structure and the inner working much more clearly. Along with this, I got to learn about smart pointers, memory management, code practices and conventions too 😄.

That makes map server really interesting.

I agree, but the entire navigation stack is one beautiful piece of art and it just keeps getting better 🌟