3732 costmap raw update msgs by emsko · Pull Request #3948 · ros-navigation/navigation2

emsko · 2023-11-08T15:54:55Z

Basic Info

Info	Please fill out this column
Ticket(s) this addresses	#3732
Primary OS tested on	Ubuntu
Robotic platform tested on	gazebo simulation

Description of contribution in a few bullet points

Adding new msg type CostmapUpdate.msg,
Adding CostmapUpdate publisher to Costmap2DPublisher class,
Re-implementation of the CostmapSubscriber class to support the CostmapUpdate msg.

Description of documentation updates required from your changes

Future work that may be required in bullet points

I see a lot of redundancy in Costmap2DPublisher class while creating the msgs, adding methods that are responsible for populating msgs and return the unique ptr to msgs might improve the code readability

For Maintainers:

Check that any new parameters added are updated in navigation.ros.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

nav2_costmap_2d/include/nav2_costmap_2d/costmap_2d_publisher.hpp

nav2_costmap_2d/src/costmap_subscriber.cpp

harderthan · 2023-11-09T13:31:17Z

nav2_costmap_2d/src/costmap_2d_publisher.cpp

 }

-void Costmap2DPublisher::publishCostmap()
+std::unique_ptr<nav2_msgs::msg::CostmapUpdate> Costmap2DPublisher::get_raw_costmap_update_msg()


I was just wondering if you could kindly explain why you use std::unique_ptr for the return type.
Returning the variable with std::unique_ptr is unnecessary due to Return Value Optimization (RVO).

Publishing the std::unique_ptr instead of raw msg is a principle adopted in this project, so that is why I've decided to create msg in unique_ptr directly.
Change publishers to publish unique ptrs

I believe that the cause for that is related with Publisher class implementation.
When calling publish(const T & msg) additional copy of msg is made when using the intra-process communication
publisher.hpp:L278, and in the end you have a copy stored in the unique_ptr and pass the ownership further.
When using publish(std::unique_ptr<MessageT, MessageDeleter> msg) you are omitting this copy. Also please take a look at the comments describing each of the overloaded methods.

SteveMacenski

Overall, very few notes, well done!

This does require some unit testing to make sure it works as expected 😄

nav2_costmap_2d/src/footprint_collision_checker.cpp

nav2_msgs/msg/CostmapUpdate.msg

SteveMacenski · 2023-11-09T16:59:50Z

nav2_costmap_2d/src/costmap_2d_publisher.cpp

      costmap_update_pub_->publish(std::move(update));
    }
+    if (costmap_raw_update_pub_->get_subscription_count() > 0) {
+      costmap_raw_update_pub_->publish(createRawCostmapUpdateMsg());


Always do std::move() on unique pointers when publishing for explicit ownership semantics. It helps with topic jitter, especially for larger topics like this

In this case, I believe that using std::move() explicitly might cause an unnecessary call to the move constructor of unique_ptr.
I have run a toy example which showed that due to RVO/NRVO optimization, no additional call to the constructor was made. However, when calling std::move() I triggered the move constructor.
When I turned off the optimization I also had a call to move constructor.

So to sum up, we are ending up with the move constructor call in the worst-case scenario, but when calling move, it is more likely, because it seems like it blocks RVO.

nav2_costmap_2d/src/costmap_2d_publisher.cpp

nav2_costmap_2d/src/costmap_subscriber.cpp

Co-authored-by: Steve Macenski <stevenmacenski@gmail.com>

…ation2 into 3732_costmap_raw_update_msgs

SteveMacenski · 2023-11-10T17:11:48Z

@emsko please don't hit "resolve" on things that haven't been pushed / completed yet. I went through and unresolved a number of items that haven't been addressed yet. It helps with my tracking of status 😄

SteveMacenski · 2023-11-15T01:14:39Z

Try rebasing and pushing a new PR with a new branch. Something is not quite right here the fact that some of your commits are not appearing on github's diff UX. Everytime that's happened in the past and I overlooked it as a quirk of github messing something up, there ended up being real issues that bit us in the butt later.

I agree the commits you link to in the github review comments appear to fix it, but the fact that I can't see that points to something being a little fishy which I want to make sure is resolved.

emsko · 2023-11-15T14:08:03Z

@SteveMacenski I have opened a new PR with rebased changes as you asked: #3965 , so hopefully the issues with Github are resolved now.
Shall we close this PR and move with the review to the new one?
I am still working on tests so there are going to be a couple more commits.

SteveMacenski · 2023-11-15T18:23:06Z

Yes, sounds good! I see things properly now in the new PR

…) (#3965) * Adding CostmapUpdate msg * CostmapRawUpdate Publisher * remove test logs and typos * change data type to uint8 * pass unique ptr to raw costmap update publisher * adding subsriber for updates * -Werror=type-limits for update lower bounds checking * copy update msg * change code formatting * adding lock guards on costmap in footprint_collision_checker.cpp * adding lock guards for costmap subscriber clients * review suggestions implementation * fixing gtests errors * changes after second round of review * Update nav2_msgs/msg/CostmapUpdate.msg Co-authored-by: Steve Macenski <stevenmacenski@gmail.com> * Integration test for costmap subscriber draft * Change the current grid parameters in separate method * WIP int test * adding method in PublisherCostmap2D for OccupancyGridUpdate population * test full costmap and updates while generating results * Integration tests for subscribers * Expected number of msgs related to number of mapchanges in tests * next round of reviews * refactor names of Costmap2DPublisher * remove unnecessary costmap_received_ flag form CostmapSubscriber --------- Co-authored-by: Steve Macenski <stevenmacenski@gmail.com>

…-navigation#3948) (ros-navigation#3965) * Adding CostmapUpdate msg * CostmapRawUpdate Publisher * remove test logs and typos * change data type to uint8 * pass unique ptr to raw costmap update publisher * adding subsriber for updates * -Werror=type-limits for update lower bounds checking * copy update msg * change code formatting * adding lock guards on costmap in footprint_collision_checker.cpp * adding lock guards for costmap subscriber clients * review suggestions implementation * fixing gtests errors * changes after second round of review * Update nav2_msgs/msg/CostmapUpdate.msg Co-authored-by: Steve Macenski <stevenmacenski@gmail.com> * Integration test for costmap subscriber draft * Change the current grid parameters in separate method * WIP int test * adding method in PublisherCostmap2D for OccupancyGridUpdate population * test full costmap and updates while generating results * Integration tests for subscribers * Expected number of msgs related to number of mapchanges in tests * next round of reviews * refactor names of Costmap2DPublisher * remove unnecessary costmap_received_ flag form CostmapSubscriber --------- Co-authored-by: Steve Macenski <stevenmacenski@gmail.com> Signed-off-by: gg <josho.wallace@gmail.com>

…-navigation#3948) (ros-navigation#3965) * Adding CostmapUpdate msg * CostmapRawUpdate Publisher * remove test logs and typos * change data type to uint8 * pass unique ptr to raw costmap update publisher * adding subsriber for updates * -Werror=type-limits for update lower bounds checking * copy update msg * change code formatting * adding lock guards on costmap in footprint_collision_checker.cpp * adding lock guards for costmap subscriber clients * review suggestions implementation * fixing gtests errors * changes after second round of review * Update nav2_msgs/msg/CostmapUpdate.msg Co-authored-by: Steve Macenski <stevenmacenski@gmail.com> * Integration test for costmap subscriber draft * Change the current grid parameters in separate method * WIP int test * adding method in PublisherCostmap2D for OccupancyGridUpdate population * test full costmap and updates while generating results * Integration tests for subscribers * Expected number of msgs related to number of mapchanges in tests * next round of reviews * refactor names of Costmap2DPublisher * remove unnecessary costmap_received_ flag form CostmapSubscriber --------- Co-authored-by: Steve Macenski <stevenmacenski@gmail.com> Signed-off-by: enricosutera <enricosutera@outlook.com>

…-navigation#3948) (ros-navigation#3965) * Adding CostmapUpdate msg * CostmapRawUpdate Publisher * remove test logs and typos * change data type to uint8 * pass unique ptr to raw costmap update publisher * adding subsriber for updates * -Werror=type-limits for update lower bounds checking * copy update msg * change code formatting * adding lock guards on costmap in footprint_collision_checker.cpp * adding lock guards for costmap subscriber clients * review suggestions implementation * fixing gtests errors * changes after second round of review * Update nav2_msgs/msg/CostmapUpdate.msg Co-authored-by: Steve Macenski <stevenmacenski@gmail.com> * Integration test for costmap subscriber draft * Change the current grid parameters in separate method * WIP int test * adding method in PublisherCostmap2D for OccupancyGridUpdate population * test full costmap and updates while generating results * Integration tests for subscribers * Expected number of msgs related to number of mapchanges in tests * next round of reviews * refactor names of Costmap2DPublisher * remove unnecessary costmap_received_ flag form CostmapSubscriber --------- Co-authored-by: Steve Macenski <stevenmacenski@gmail.com>

emsko added 11 commits September 6, 2023 16:09

Adding CostmapUpdate msg

387404b

CostmapRawUpdate Publisher

883b3d3

remove test logs and typos

9a0d395

change data type to uint8

bd86788

pass unique ptr to raw costmap update publisher

d768f71

adding subsriber for updates

7083de9

-Werror=type-limits for update lower bounds checking

e15c22e

copy update msg

500be4b

change code formatting

ad01f6e

adding lock guards on costmap in footprint_collision_checker.cpp

addd825

adding lock guards for costmap subscriber clients

2440f7d

harderthan reviewed Nov 9, 2023

View reviewed changes

review suggestions implementation

c21554a

SteveMacenski requested changes Nov 9, 2023

View reviewed changes

emsko and others added 2 commits November 9, 2023 19:35

Update nav2_msgs/msg/CostmapUpdate.msg

db0b9d6

Co-authored-by: Steve Macenski <stevenmacenski@gmail.com>

fixing gtests errors

31d1957

emsko mentioned this pull request Nov 10, 2023

Costmap2DPublisher refactoring #3956

Closed

emsko added 2 commits November 10, 2023 18:08

changes after second round of review

33bf135

Merge branch '3732_costmap_raw_update_msgs' of github.com:emSko/navig…

f163e0a

…ation2 into 3732_costmap_raw_update_msgs

Integration test for costmap subscriber draft

5b7793f

SteveMacenski closed this Nov 15, 2023

SteveMacenski reopened this Nov 15, 2023

emsko mentioned this pull request Nov 15, 2023

3732 costmap raw update msgs rebased (fixes github issues with PR #3948) #3965

Merged

7 tasks

SteveMacenski closed this Nov 15, 2023

Conversation

emsko commented Nov 8, 2023

Basic Info

Description of contribution in a few bullet points

Description of documentation updates required from your changes

Future work that may be required in bullet points

For Maintainers:

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

harderthan Nov 9, 2023

Choose a reason for hiding this comment

Uh oh!

emsko Nov 10, 2023

Choose a reason for hiding this comment

Uh oh!

SteveMacenski left a comment

Choose a reason for hiding this comment

Uh oh!

Uh oh!

Uh oh!

Uh oh!

SteveMacenski Nov 9, 2023

Choose a reason for hiding this comment

Uh oh!

emsko Nov 14, 2023

Choose a reason for hiding this comment

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

SteveMacenski commented Nov 10, 2023

Uh oh!

SteveMacenski commented Nov 15, 2023 • edited Loading Uh oh! There was an error while loading. Please reload this page.

Uh oh!

Uh oh!

emsko commented Nov 15, 2023

Uh oh!

SteveMacenski commented Nov 15, 2023

Uh oh!

Reviewers

Assignees

Labels

Projects

Milestone

Development

Uh oh!

3 participants

SteveMacenski commented Nov 15, 2023 •

edited

Loading