Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Add Vector Object server #4680

Open
wants to merge 22 commits into
base: main
Choose a base branch
from
Open

Conversation

ajtudela
Copy link
Contributor


Basic Info

Info Please fill out this column
Ticket(s) this addresses #3017, #3930
Primary OS tested on Ubuntu 24.04
Robotic platform tested on
Does this PR contain AI generated software? No

Description of contribution in a few bullet points

Just the commits from #3930

Description of documentation updates required from your changes


Future work that may be required in bullet points

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

AlexeyMerzlyakov and others added 20 commits September 17, 2024 14:27
Signed-off-by: Alexey Merzlyakov <[email protected]>
Signed-off-by: Alberto Tudela <[email protected]>
Signed-off-by: Alexey Merzlyakov <[email protected]>
Signed-off-by: Alexey Merzlyakov <[email protected]>
Signed-off-by: Alberto Tudela <[email protected]>
Signed-off-by: Alexey Merzlyakov <[email protected]>
Signed-off-by: Alberto Tudela <[email protected]>
Signed-off-by: Alexey Merzlyakov <[email protected]>
Signed-off-by: Alberto Tudela <[email protected]>
Signed-off-by: Alexey Merzlyakov <[email protected]>
Signed-off-by: Alexey Merzlyakov <[email protected]>
Signed-off-by: Alberto Tudela <[email protected]>
Co-authored-by: Steve Macenski <[email protected]>
Signed-off-by: Alexey Merzlyakov <[email protected]>
Signed-off-by: Alberto Tudela <[email protected]>
Signed-off-by: Alexey Merzlyakov <[email protected]>
Signed-off-by: Alberto Tudela <[email protected]>
Signed-off-by: Alexey Merzlyakov <[email protected]>
Signed-off-by: Alberto Tudela <[email protected]>
Signed-off-by: Alexey Merzlyakov <[email protected]>
Signed-off-by: Alberto Tudela <[email protected]>
Signed-off-by: Alexey Merzlyakov <[email protected]>
Signed-off-by: Alberto Tudela <[email protected]>
Signed-off-by: Alexey Merzlyakov <[email protected]>
Signed-off-by: Alberto Tudela <[email protected]>
Signed-off-by: Alexey Merzlyakov <[email protected]>
Signed-off-by: Alberto Tudela <[email protected]>
Signed-off-by: Alexey Merzlyakov <[email protected]>
Signed-off-by: Alberto Tudela <[email protected]>
Signed-off-by: Alexey Merzlyakov <[email protected]>
Signed-off-by: Alberto Tudela <[email protected]>
Signed-off-by: Alexey Merzlyakov <[email protected]>
Signed-off-by: Alberto Tudela <[email protected]>
* Corrected headers
* Functions ordering
* Comment fixes

Signed-off-by: Alexey Merzlyakov <[email protected]>
Signed-off-by: Alberto Tudela <[email protected]>
* Correct licensing years
* Fix Vector Object server dependencies
* Funcion rename for better readability
* Improve/fix comments

Signed-off-by: Alexey Merzlyakov <[email protected]>
Signed-off-by: Alberto Tudela <[email protected]>
@SteveMacenski SteveMacenski mentioned this pull request Sep 17, 2024
7 tasks
@ajtudela
Copy link
Contributor Author

ajtudela commented Nov 7, 2024

Hi @SteveMacenski, if you have a moment, could you look at this and let me know what changes are needed.

Thanks very much!

@SteveMacenski
Copy link
Member

SteveMacenski commented Nov 13, 2024

I took a look at about ~2/3 of it this afternoon. I didn't quite get to all of it, so I'll finish my review next time I get a chance :-)

In glancing through the old PR, I came up on this comment #3930 (comment) -- what do you think?

Copy link
Member

@SteveMacenski SteveMacenski left a comment

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Done! Largely nitpicks :-) Can you go through the annotations for test coverage where missing and fill in a few of the gaps that are relatively easy to do?

'container_name', default_value='nav2_container',
description='the name of conatiner that nodes will load in if use composition')

configured_params = ParameterFile(
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Check this against main: is this still how we handle this? I think this may have changed

Copy link
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I checked and it's the same in other launch files in main.

PushROSNamespace(
condition=IfCondition(NotEqualsSubstitution(LaunchConfiguration('namespace'), '')),
namespace=namespace),
Node(
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Put the lifecycle manager last so it doesn't try to bring servers up that aren't yet initialized. Ditto in the composition version below


bool VectorObjectServer::obtainParams()
{
// Main ROS-parameters
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Usually we do a auto node = shared_from_this() so that the node is used in all the calls below (its more natural looking)

shared_from_this(), "shapes", std::vector<std::string>()).as_string_array();
for (std::string shape_name : shape_names) {
std::string shape_type;

Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Remove line

}
shapes_.push_back(circle);
} else {
RCLCPP_ERROR(get_logger(), "Please specify correct shape %s type", shape_name.c_str());
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

In the log, give the options so they can fix it fast


for (auto shape : shapes_) {
shape->getBoundaries(min_p_x, min_p_y, max_p_x, max_p_y);
if (min_p_x < min_x) {
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

std::min and std::max would be more concise

if (!map_) {
map_ = std::make_shared<nav_msgs::msg::OccupancyGrid>();
}
if (
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Add space between if statements


void VectorObjectServer::publishMap()
{
if (map_) {
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Check also if map_pub_'s number of subscribers is > 0 to bother with the operation at all

publishMap();
}

void VectorObjectServer::switchMapUpdate()
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I am a little baffled on this method.

Do you have an idea why we create a map_timer_ for each object who's frame ID doesn't match the current frame ID? It seems to override each other. We then instantly cancel it if it exists too... and processMap

Copy link
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I'm not exactly sure. I read this in the doc PR:

  • If at least one of the shape is set in different than map's frame, dynamic update model to be enabled: this shape can move over the time, output map will be published dynamically with a given rate.
  • If all shapes are set in the same as map frame, map is being published/updated once: during Vector Object server startup and per each shape changing call (AddShapes.srv or RemoveShapes.srv).

max_y = std::numeric_limits<double>::lowest();

for (auto point : polygon_->points) {
if (point.x < min_x) {
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

std min/max

Signed-off-by: Alberto Tudela <[email protected]>
Signed-off-by: Alberto Tudela <[email protected]>
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
Labels
None yet
Projects
None yet
Development

Successfully merging this pull request may close these issues.

3 participants