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

Use SetParameter Launch API to set the yaml filename for map_server #3174

Merged
merged 2 commits into from
Nov 7, 2022

Conversation

stevedanomodolor
Copy link
Contributor

@stevedanomodolor stevedanomodolor commented Sep 6, 2022


Basic Info

Info Please fill out this column
Ticket(s) this addresses (add tickets here #3149 )
Primary OS tested on (Ubuntu)
Robotic platform tested on (gazebo)

Description of contribution in a few bullet points

  • Use SetParameter Launch API to set the yaml filename for map_server

Description of documentation updates required from your changes

  • Remove the note added to the navigation.ros.org configuration for map server explaining that yaml_filename is strictly required.

Future work that may be required in bullet points

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

@mergify
Copy link
Contributor

mergify bot commented Sep 6, 2022

@stevedanomodolor, please properly fill in PR template in the future. @SteveMacenski, use this instead.

  • 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

@stevedanomodolor
Copy link
Contributor Author

@SteveMacenski I am still not sure if a group action is required in order to use the setparameter or it can be used under the Node action directly, there is not that much documentation. I have to test it to see if it is necessary. I have not tested this commit thats why its on draft.

@mergify
Copy link
Contributor

mergify bot commented Sep 6, 2022

@stevedanomodolor, your PR has failed to build. Please check CI outputs and resolve issues.
You may need to rebase or pull in main due to API changes (or your contribution genuinely fails).

nav2_bringup/launch/localization_launch.py Show resolved Hide resolved
nav2_bringup/launch/bringup_launch.py Outdated Show resolved Hide resolved
nav2_bringup/launch/slam_launch.py Outdated Show resolved Hide resolved
nav2_bringup/launch/slam_launch.py Outdated Show resolved Hide resolved
@SteveMacenski
Copy link
Member

if this all works out - you can also remove the yaml_filename from the yaml files. Leave it there, but just comment it out and update the comment above it explaining that it doesn't need to be specified since going to be set by defaults in launch. If you'd rather set it in the yaml, remove the defaults in launch and set the full filepath here.

And subsequent updates in docs

Thanks so much for the help! And @tonynajjar for the idea!

@stevedanomodolor
Copy link
Contributor Author

@SteveMacenski @tonynajjar. i have tried setting the parameters but still get the same error

[ERROR] [launch]: Caught exception in launch (see debug for traceback): 'NoneType' object is not iterable
I am supposed to declare the yaml_filename in a specific way. There is not too much documentation on it.

@mergify
Copy link
Contributor

mergify bot commented Sep 12, 2022

@stevedanomodolor, your PR has failed to build. Please check CI outputs and resolve issues.
You may need to rebase or pull in main due to API changes (or your contribution genuinely fails).

@SteveMacenski
Copy link
Member

SteveMacenski commented Sep 12, 2022

What if yaml_filename is non-empty string? Then does it work? I'm curious if its failing because its just not set (and then we could use a launch action condition to only set the parameter if its set to something).

It doesn't seem to be that there needs to be a declaration: https://github.com/ros2/launch_ros/blob/rolling/launch_ros/launch_ros/actions/set_parameter.py#L38-L55 but it does seem to me like it might need to exist

@stevedanomodolor
Copy link
Contributor Author

stevedanomodolor commented Sep 14, 2022

What if yaml_filename is non-empty string? Then does it work? I'm curious if its failing because its just not set (and then we could use a launch action condition to only set the parameter if its set to something).

It doesn't seem to be that there needs to be a declaration: https://github.com/ros2/launch_ros/blob/rolling/launch_ros/launch_ros/actions/set_parameter.py#L38-L55 but it does seem to me like it might need to exist

I am performing all test with the tb3_simulation_launch.py with ros rolling(used for development)
What if yaml_filename is non-empty string? -> I am assuming you are referring to the yaml_filename in the parameter file.
When the yaml_filename parameter in the parameter list file is set to an empty string , the launch file execute with no map independent of if the setparameter was set or not. In this case the param substitution is not used. And when the yaml_filename parameters is not set in the parameter file and the param substitution not used, it gives the error nonetype as said previously. I also tried setting the default parameters in the launch file to non empty string but still gives the nonetype error. The set parameter does not seem to influence at all the behavior.
I am assuming that the underlining issue is the fact that the variable type is a string, in comparison to the use_sim_time which is a boolean. @tonynajjar any thought?

@SteveMacenski
Copy link
Member

SteveMacenski commented Sep 14, 2022

What if yaml_filename is non-empty string? -> I am assuming you are referring to the yaml_filename in the parameter file.

I meant in the launch file declaration, so that when you call SetParameter() there's a non-empty value to it. This is just a shot in the dark but does SetParameter(name='my_param', value='2') API work (explicitly setting name/value)? I've found some oddities like that in the past with rclpy.

I don't think the type has anything to do with it. @tonynajjar any thoughts?

@stevedanomodolor
Copy link
Contributor Author

@SteveMacenski The issue was from my side. I had an issue with the docker, I have fixed it now.

@tonynajjar
Copy link
Contributor

Great, I was about to have a look. If you have another issue, please make sure to push your code so I can try to reproduce (can't guarantee a timely response though)

@SteveMacenski
Copy link
Member

SteveMacenski commented Sep 15, 2022

@stevedanomodolor so this works?

  • With map file in the .yaml file -- uses that (if default in launch configuration is empty string)
  • Without any map defined in the .yaml file -- uses default in launch configuration

I was thinking we might need an IfCondition to apply the SetParameter only if the value of it in the launch configuration was non-empty string. I wasn't sure if an empty string launch configuration would override a real valued parameter definition in the .yaml file.

@stevedanomodolor
Copy link
Contributor Author

@stevedanomodolor so this works?

  • With map file in the .yaml file -- uses that (if default in launch configuration is empty string)
  • Without any map defined in the .yaml file -- uses default in launch configuration

I was thinking we might need an IfCondition to apply the SetParameter only if the value of it in the launch configuration was non-empty string. I wasn't sure if an empty string launch configuration would override a real valued parameter definition in the .yaml file.

Scenario 1 -> works
Scenario 2 -> works -> current commit
The yaml_filename when defined in the .yaml always overrides the launch file.
Sorry for this taking longer than it should, I had some issue with the docker version

@SteveMacenski
Copy link
Member

Hey, no worries. Its all a process 😄

The yaml_filename when defined in the .yaml always overrides the launch file.

Oh really? So if we have yaml_filename: "" in .yaml, it'll override the map launch configuration that has some real-existing default value location? I would have thought that the launch configuration would override the yaml file value for any parameter when you call SetParameter().

I'm trying to game out what I would expect to happen in the major situations:

  • No launch default + in yaml -> use yaml
  • Launch default or set on commandline + in yaml -> use launch default or value on commandline
  • No launch default + not in yaml -> completely unset
  • Launch default or set on commandline + not in yaml -> use launch default or value on commandline

I think you're saying that the 2nd case is the opposite? I suppose that is fine if that's the way this acts, but that seems suspicious to me that a launch-set parameter would not take priority over a yaml set one.

@stevedanomodolor
Copy link
Contributor Author

Launch default or set on commandline + in yaml -> use launch default or value on commandline
launch default + yaml -> it pays attention to yaml. I am not if it does that because of the way configured_params loads the yaml after the setparameter has been called. This might be a better suggestion I think, I am not sure.

I was thinking we might need an IfCondition to apply the SetParameter only if the value of it in the launch configuration was non-empty string. I wasn't sure if an empty string launch configuration would override a real valued parameter definition in the .yaml file.

@SteveMacenski
Copy link
Member

SteveMacenski commented Sep 19, 2022

Launch default or set on commandline + in yaml ...
launch default + yaml ...

Aren't these the same?

If both in yaml and launch configuration, I would expect it to use the launch override, is there a way to accomplish that? If not, maybe we can document the fact that yaml takes over and only set in the yaml file if you intend not use launch overrides / defaults.

@stevedanomodolor
Copy link
Contributor Author

The main reason why I think the yaml file takes over is because it is just a redeclaration of the variable. Basically, calling the setparameter before the node is a requirement which initially declares the variable but the yaml file is added to the node causing the redeclaring I was referring too. Probably using an ifcondition to see the state of the variable and based on that redeclare with the yaml might be an alternative. i could try that

@SteveMacenski
Copy link
Member

SteveMacenski commented Sep 21, 2022

It would be nice to experiment to see if that's possible! But ultimately, if its not, I think we can live with it. We just need to make sure we clearly document it in a few places to ward off confusion. Ex. in the .yaml file above the commented out yaml_filename comment block, in the Nav2 documentation for that parameter, and as a comment in localization.yaml. Basically, mention it anywhere where someone would try to investigate.


@clalancette Is this expected behavior? When we use SetParameter() in launch and the same parameter exists in the params.yaml file, the value in params.yaml is used rather than the launch's SetParameter() value which was set via CLI. My intuition would say that launch-time overrides should take precedence, but that's not the behavior we experience. If that is expected behavior, please let us know (and I'll open a PR to document that somewhere).

@clalancette
Copy link
Contributor

@clalancette Is this expected behavior? When we use SetParameter() in launch and the same parameter exists in the params.yaml file, the value in params.yaml is used rather than the launch's SetParameter() value which was set via CLI. My intuition would say that launch-time overrides should take precedence, but that's not the behavior we experience. If that is expected behavior, please let us know (and I'll open a PR to document that somewhere).

I'm not really an expert on what launch does. But here's what I found when playing around with the command-line.

For instance, if I have the following parameters in a file called cam2image.yaml:

/cam2image:
  ros__parameters:
    burger_mode: true
    depth: 10
    device_id: 0
    frame_id: bar
    frequency: 30.0
    height: 240
    history: keep_all
    qos_overrides:
      /parameter_events:
        publisher:
          depth: 1000
          durability: volatile
          history: keep_last
          reliability: reliable
    reliability: reliable
    show_camera: false
    use_sim_time: false
    width: 320

And I run:

ros2 run image_tools cam2image --ros-args --params-file cam2image.yaml

Then the frame_id is set to bar, like we expect.

If I then run:

ros2 run image_tools cam2image --ros-args --params-file cam2image.yaml -p frame_id:=foo

Then the frame_id is overridden to foo, also like we expect. So the command-line seems to be working properly.

To move forward here, I think it would be worthwhile to see what the command-line and parameters file that launch generates look like. That may give us a better idea of where the precedence is being set.

@SteveMacenski
Copy link
Member

SteveMacenski commented Sep 28, 2022

Related, but not quite the same thing as what we're handling. We have a launch system with a non-trivial number of nodes, so there's no -p <a_specific_parameter>:=val, this is via ros2 launch not ros2 run (setting for a single node, not the whole system). For launch, we set specific LaunchConfiguration's which have val set, and when we SetParameter() of those launch configurations, the yaml is taking priority. That seems like the opposite behavior than when running a single node, which is not good that we have inconsistent behavior between 2 analog processes

An exact analog would be if we set parameters=[configured_params, {'yaml_filename': val}], in the launch Node() function instead of using the SetParameter() launch function. That would create a conflict because the parameter exists in both the yaml and set individually, but if we should expect that this would have the same policy as ros2 run we'd expect that to use the individual parameter override, yes? @stevedanomodolor can you test this?

If that works, that solves our PR needs. But that still leaves a problem outstanding that the SetParameter() method of launch either (a) doesn't handle conflicts as it should (b) it does as it should, but needs to document that policy, or (c) needs to have the option made available to override. I would prefer A or C, but if that's the way folks want it and just needs documentation (B), I can try to see where that would be sensible and do so.


ros2 run image_tools cam2image --ros-args

Here nor there, but is the reverse true, if -p frame_id:=foo --params-file cam2image.yaml instead? Wondering if the reason that works is due to ordering of arguments or a specific policy of setting individual parameters over parameter file values.

@stevedanomodolor
Copy link
Contributor Author

stevedanomodolor commented Sep 28, 2022

@SteveMacenski

Assuming we used the following yaml file

map_server:
  ros__parameters:
    yaml_filename: "bar"

I replaced all the launch variable name 'map' in the launch file to 'yaml_filename' to make it easier to understand
The normal situation without commenting the map_server in the yaml file

ros2 launch nav2_bringup tb3_simulation_launch.py headless:=False 
ros2 param get /map_server yaml_filename

This gives
String value is: bar
For the second situation, I modified the yaml_filename parameters from the launch argument

ros2 launch nav2_bringup tb3_simulation_launch.py headless:=False yaml_filename:=test
ros2 param get /map_server yaml_filename

This gives
String value is: bar
The result makes sense because in the end we are just modifying the launch parameter, nothing to do with the node

Now, equivalent to the second example you mentioned @clalancette
parameters=[configured_params, {'yaml_filename': map_yaml_file}],

ros2 launch nav2_bringup tb3_simulation_launch.py headless:=False yaml_filename:=test
ros2 param get /map_server yaml_filename

This gives
String value is: test
This still means that the SetParameter declares but the fact that configured_params is added to the parameters of the map server node, the value in the yaml file overrides that declaration, and {'yaml_filename': map_yaml_file} overrides the yaml file.
In the last case, the yaml file is always overridden and redundant and the yaml_filename must be set in the CLI or launch file. This is equivalent to removing the yaml from the loop entirely and just setting through CLI or launch file

@SteveMacenski
Copy link
Member

SteveMacenski commented Sep 29, 2022

Does that make sense @clalancette? SetParameter() in Launch only works if never otherwise set, which is very not intuitive and not consistent with the way that ros2 run (as you showed) or manually setting in a launch file's conflicting parameters=[] field handles things (as @stevedanomodolor showed).

Should I file a ticket about this?


@stevedanomodolor So for a path forward here, it sounds like we should:

  • Add parameters=[configured_params, {'yaml_filename': map_yaml_file}], if map_yaml_file is non-empty string, rather than use SetParameter
  • Its not super clean, but that should be possible if we have 2 instances of the Node() for map server, one with a condition=IfCondition(PythonExpression(['not ', map_yaml_file])), to not set the map, and one with condition=IfCondition(PythonExpression([map_yaml_file, ' != ""'])), to set the map (representing that the map is non-empty)

That way, we get:

  • If not set on launch commandline or by a launch configuration default, we don't override a yaml default
  • If we set it on launch commandline / launch configuration, we do override a yaml default
  • If no yaml default exists and not set, throws
  • If no yaml is set and anywhere it is set, it uses the new value

I think that gets us what we want, yes? I don't see a cleaner way to have a condition for including a parameter field to the parameter=[] list (unless empty-string for some rule doesn't override a yaml default automatically, but I don't suspect that's true). I'd just add in a 1-line comment about why this was necessary for future users.

@stevedanomodolor
Copy link
Contributor Author

stevedanomodolor commented Oct 5, 2022

@SteveMacenski It seems that comparison with variables that are not either boolean or numbers does not work

condition=IfCondition(PythonExpression([map_yaml_file, ' != ""'])) condition=IfCondition(PythonExpression(['not ', map_yaml_file]))

It constantly gives the error
[ERROR] [launch]: Caught exception in launch (see debug for traceback): invalid syntax (<string>, line 1)

It seems that pythonExpression does not work with string comparison only numbers and boolean

@SteveMacenski
Copy link
Member

SteveMacenski commented Oct 5, 2022

Huh, in googling looks like there's new completely undocumented or described LaunchConfigurationEquals and LaunchConfigurationNotEquals that should do the trick!

https://answers.ros.org/question/360264/ifconditionpythonexpression-am-i-doing-it-right/

@codecov
Copy link

codecov bot commented Oct 12, 2022

Codecov Report

Base: 82.62% // Head: 82.60% // Decreases project coverage by -0.01% ⚠️

Coverage data is based on head (5d69f12) compared to base (c5de9b9).
Patch has no changes to coverable lines.

❗ Current head 5d69f12 differs from pull request most recent head a2a2af4. Consider uploading reports for the commit a2a2af4 to get more accurate results

Additional details and impacted files
@@            Coverage Diff             @@
##             main    #3174      +/-   ##
==========================================
- Coverage   82.62%   82.60%   -0.02%     
==========================================
  Files         340      340              
  Lines       15410    15410              
==========================================
- Hits        12732    12729       -3     
- Misses       2678     2681       +3     
Impacted Files Coverage Δ
...stmap_2d/plugins/costmap_filters/binary_filter.cpp 94.80% <0.00%> (-2.60%) ⬇️
..._amcl/src/sensors/laser/likelihood_field_model.cpp 95.00% <0.00%> (-2.50%) ⬇️

Help us with your feedback. Take ten seconds to tell us how you rate us. Have a feature suggestion? Share it here.

☔ View full report at Codecov.
📢 Do you have feedback about the report comment? Let us know in this issue.

@stevedanomodolor stevedanomodolor marked this pull request as ready for review October 12, 2022 21:19
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.

Small nit and ready! Please also open a PR against our documentation to add to the migration guide this change!

nav2_bringup/params/nav2_params.yaml Outdated Show resolved Hide resolved
minor fix

fix commit

reincluded rewritten function

comment remaining lines for yaml_filename

removed default_value

issue with composable node

alternative soltion to the condition param not working in composable node

remove unused import

remove comments and reorder composablenode execution

fixing commit

fixing format

fixing lint

Update nav2_bringup/params/nav2_params.yaml

Co-authored-by: Steve Macenski <stevenmacenski@gmail.com>
@stevedanomodolor
Copy link
Contributor Author

stevedanomodolor commented Oct 23, 2022

@SteveMacenski
Copy link
Member

Hi,

Sorry for the delay (roscon/iros), I meant adding to the migration guide this behavior has changed https://github.com/ros-planning/navigation.ros.org/blob/master/migration/Humble.rst. Your updates to the parameter guide though are very helpful and merged those!

@SteveMacenski
Copy link
Member

ros2/launch_ros#328 The folks at OR responded to your launch issue - can you try out @methylDragon 's suggestion?

@stevedanomodolor
Copy link
Contributor Author

ros2/launch_ros#328 The folks at OR responded to your launch issue - can you try out @methylDragon 's suggestion?

I have added the changes to the migration guide. Wrt to ros2/launch_ros#328, it seems that the feature was added not long ago, when i try to update the launch package, I do not get that update, currently using the docker form of nav2, I an not sure if that is the reason why it does not get those changes. If i get that resolved, I will try it, if you know a way to solve this, let me know, always happy to hear your feedback.

@SteveMacenski
Copy link
Member

OK, might be something good to revisit then once things sync up to Rolling.

Can you make a note about that in that comment block so next time I stumble into it in the future, I can test it out?

Reviewed the migration guide PR, just small stuff and then we can merge the pair of PRs.

Thanks for your patience and help with this!

@stevedanomodolor
Copy link
Contributor Author

When you mean comment block, are you referring to the migration guide ?, btw I already made a code to test this new feature: https://github.com/stevedanomodolor/navigation2/tree/feat/EqualsSubstitution, if it might be useful if you want to test it too.

@SteveMacenski
Copy link
Member

Thanks! Its not yet released yet from MenthylDragon's response. So, that's a future thing in a few weeks, but I don't want to block this PR for that. I can apply your patch later on.

I mean here: https://github.com/ros-planning/navigation2/pull/3174/files#diff-a76c17e1db2110fbb385675216aa5a94bba6f06a2c5744b38621ae6b5e526c27R158 add to the comment block in the launch file also a note about switching to use the new method because the current one is scheduled for deprecation once a Rolling sync is conducted.

@stevedanomodolor
Copy link
Contributor Author

Thanks! Its not yet released yet from MenthylDragon's response. So, that's a future thing in a few weeks, but I don't want to block this PR for that. I can apply your patch later on.

I mean here: https://github.com/ros-planning/navigation2/pull/3174/files#diff-a76c17e1db2110fbb385675216aa5a94bba6f06a2c5744b38621ae6b5e526c27R158 add to the comment block in the launch file also a note about switching to use the new method because the current one is scheduled for deprecation once a Rolling sync is conducted.

Changes added

@SteveMacenski
Copy link
Member

Just waiting on CI

@SteveMacenski SteveMacenski merged commit a03cb63 into ros-navigation:main Nov 7, 2022
jwallace42 pushed a commit to jwallace42/navigation2 that referenced this pull request Nov 30, 2022
…os-navigation#3174)

* implement launch priority for the mapserver parameter yaml_filename

minor fix

fix commit

reincluded rewritten function

comment remaining lines for yaml_filename

removed default_value

issue with composable node

alternative soltion to the condition param not working in composable node

remove unused import

remove comments and reorder composablenode execution

fixing commit

fixing format

fixing lint

Update nav2_bringup/params/nav2_params.yaml

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

* state new ros-rolling release changes and deprecation

Co-authored-by: Steve Macenski <stevenmacenski@gmail.com>
SteveMacenski added a commit that referenced this pull request Dec 8, 2022
* publish layers of costmap

* lint fix

* lint round 2 :)

* code review

* remove isPublishable

* lint

* test running

* rough structure complete

* completed test

* lint

* code review

* CI

* CI

* linting

* completed pub test

* CostmapLayer::matchSize may be executed concurrently (#3250)

* CostmapLayer::matchSize() add a mutex

* Fix typo (#3262)

* Adding new Nav2 Smoother: Savitzky-Golay Smoother (#3264)

* initial prototype of the Savitzky Golay Filter Path Smoother

* fixed indexing issue - tested working

* updates for filter

* adding unit tests for SG-filter smoother

* adding lifecycle transitions

* refactoring RPP a bit for cleanliness on way to ROSCon (#3265)

* refactor for RPP on way to ROSCon

* fixing header

* fixing header

* fixing header

* fix edge cases test samplings

* linting

* exceptions for compute path through poses (#3248)

* exceptions for compute path through poses

* lint fix

* code review

* code review

Co-authored-by: Joshua Wallace <josho.wallace.com>

* Reclaim Our CI Coverage from the Lords of Painful Subtle Regressions ⚔️⚔️⚔️ (#3266)

* test waypoint follower with composition off for logging

* adding no composition to all system tests

* Added Line Iterator (#3197)

* Added Line Iterator

* Updated Line Iterator to a new iteration method

* Added the resolution as a parameter/ fixed linting

* Added the resolution as a parameter/ fixed linting

* Added unittests for the line iterator

* Added unittests based on "unittest" package

* Fixed __init__.py and rephrased some docstrings

* Fixed linting errors

* Fixed Linting Errors

* Added some unittests and removed some methods

* Dummy commit for CircleCI Issue

Co-authored-by: Afif Swaidan <afif.swaidan@spexal.com>

* Use SetParameter Launch API to set the yaml filename for map_server (#3174)

* implement launch priority for the mapserver parameter yaml_filename

minor fix

fix commit

reincluded rewritten function

comment remaining lines for yaml_filename

removed default_value

issue with composable node

alternative soltion to the condition param not working in composable node

remove unused import

remove comments and reorder composablenode execution

fixing commit

fixing format

fixing lint

Update nav2_bringup/params/nav2_params.yaml

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

* state new ros-rolling release changes and deprecation

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

* adding reconfigure test to thetastar (#3275)

* Check for range_max of laserscan in updateFilter to avoid a implicit overflow crash. (#3276)

* Update amcl_node.cpp

* fit the code style

* fit code style

* BT Service Node to throw if service was not available in time (#3256)

* throw if service server wasn't available in time

mimic the behavior of the bt action node constructor

* throw if action unavailable in bt cancel action

* use chrono literals namespace

* fix linting errors

* fix code style divergence

* remove exec_depend on behaviortree_cpp_v3 (#3279)

* add parameterized refinement recursion numbers in Smac Planner Smoother and Simple Smoother (#3284)

* add parameterized refinement recursion numbers

* fix tests

* Add allow_unknown parameter to theta star planner (#3286)

* Add allow unknown parameter to theta star planner

* Add allow unknown parameter to tests

* missing comma

* Change cost of unknown tiles

* Uncrustify

* Include test cases waypoint follwer (#3288)

* WIP

* included missed_waypoing check

* finished inclding test

* fix format

* return default sleep value

* Dynamically changing polygons support (#3245)

* Add Collision Monitor polygon topics subscription

* Add the support of polygons published in different frame

* Internal review

* Fix working with polygons visualization

* Update nav2_collision_monitor/README.md

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

* Move getTransform to nav2_util

* Fix misprint

* Meet remaining review items:
* Update polygon params handling logic
* Warn if polygon shape was not set
* Publish with ownership movement

* Correct polygons_test.cpp parameters handling logic

* Adjust README for dynamic polygons logic update

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

* adding getCostScalingFactor (#3290)

* Implemented smoother selector bt node (#3283)

* Implemented smoother selector bt node

Signed-off-by: Owen Hooper <17ofh@queensu.ca>

* updated copyright in modified file

Signed-off-by: Owen Hooper <17ofh@queensu.ca>

Signed-off-by: Owen Hooper <17ofh@queensu.ca>

* Pipe error codes (#3251)

* issue with finding key

* passed up codes to bt_navigator

* lint fix

* updates

* adding error_code_id back in

* error codes names in params

* bump error codes

* lint

* spelling

* test fix

* update behavior trees

* cleanup

* Update bt_action_server_impl.hpp

* code review

* lint

* code review

* log fix

* error code for waypoint follower

* clean up

* remove waypoint error test, too flaky on CI

* lint and code review

* rough imp for waypoint changes

* lint

* code review

* build fix

* clean up

* revert

* space

* remove

* try to make github happ

* stop gap

* loading in param file

* working tests :)

* lint

* fixed cmake

* lint

* lint

* trigger build

* added invalid plugin error

* added test for piping up error codes

* clean up

* test waypoint follower

* only launch what is needed

* waypoint test

* revert lines for robot navigator

* fix test

* waypoint test

* switched to uint16

* clean up

* code review

* todo to note

* lint

* remove comment

* Update nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server_impl.hpp

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

* rename error_codes

* error code for navigate to pose

* error codes for navigate through poses.

* error codes for navigate through poses

* message update for waypoint follower

* rename to error code

* update node xml

Co-authored-by: Joshua Wallace <josho.wallace.com>
Co-authored-by: Steve Macenski <stevenmacenski@gmail.com>

* Solve bug when CostmapInfoServer is reactivated (#3292)

* Solve bug when CostmapInfoServer is reactivated

* Smoothness metrics update (#3294)

* Update metrics for path smoothness

* Support Savitzky-Golay smoother

* preempt/cancel test for time behavior, spin pluguin (#3301)

* include preempt/cancel test for time behavior, spin pluguin

* linting

* fix bug in code

* lint fix

* clean up test

* lint

* cleaned up test

* update

* revert Cmake

Signed-off-by: Owen Hooper <17ofh@queensu.ca>
Co-authored-by: Joshua Wallace <josho.wallace.com>
Co-authored-by: Hao-Xuan Song <44140526+Cryst4L9527@users.noreply.github.com>
Co-authored-by: jaeminSHIN <91681721+woawo1213@users.noreply.github.com>
Co-authored-by: Steve Macenski <stevenmacenski@gmail.com>
Co-authored-by: Afif Swaidan <53655365+afifswaidan@users.noreply.github.com>
Co-authored-by: Afif Swaidan <afif.swaidan@spexal.com>
Co-authored-by: Stevedan Ogochukwu Omodolor <61468301+stevedanomodolor@users.noreply.github.com>
Co-authored-by: Pradheep Krishna <padhupradheep@gmail.com>
Co-authored-by: Erwin Lejeune <erwin.lejeune15@gmail.com>
Co-authored-by: Adam Aposhian <aposhian.dev@gmail.com>
Co-authored-by: Pedro Alejandro González <71234974+pepisg@users.noreply.github.com>
Co-authored-by: Alexey Merzlyakov <60094858+AlexeyMerzlyakov@users.noreply.github.com>
Co-authored-by: Owen Hooper <17ofh@queensu.ca>
Co-authored-by: MartiBolet <43337758+MartiBolet@users.noreply.github.com>
SteveMacenski added a commit that referenced this pull request Dec 9, 2022
* publish layers of costmap

* lint fix

* lint round 2 :)

* code review

* remove isPublishable

* lint

* test running

* rough structure complete

* completed test

* lint

* code review

* CI

* CI

* linting

* completed pub test

* CostmapLayer::matchSize may be executed concurrently (#3250)

* CostmapLayer::matchSize() add a mutex

* Fix typo (#3262)

* Adding new Nav2 Smoother: Savitzky-Golay Smoother (#3264)

* initial prototype of the Savitzky Golay Filter Path Smoother

* fixed indexing issue - tested working

* updates for filter

* adding unit tests for SG-filter smoother

* adding lifecycle transitions

* refactoring RPP a bit for cleanliness on way to ROSCon (#3265)

* refactor for RPP on way to ROSCon

* fixing header

* fixing header

* fixing header

* fix edge cases test samplings

* linting

* exceptions for compute path through poses (#3248)

* exceptions for compute path through poses

* lint fix

* code review

* code review

Co-authored-by: Joshua Wallace <josho.wallace.com>

* Reclaim Our CI Coverage from the Lords of Painful Subtle Regressions ⚔️⚔️⚔️ (#3266)

* test waypoint follower with composition off for logging

* adding no composition to all system tests

* Added Line Iterator (#3197)

* Added Line Iterator

* Updated Line Iterator to a new iteration method

* Added the resolution as a parameter/ fixed linting

* Added the resolution as a parameter/ fixed linting

* Added unittests for the line iterator

* Added unittests based on "unittest" package

* Fixed __init__.py and rephrased some docstrings

* Fixed linting errors

* Fixed Linting Errors

* Added some unittests and removed some methods

* Dummy commit for CircleCI Issue

Co-authored-by: Afif Swaidan <afif.swaidan@spexal.com>

* Use SetParameter Launch API to set the yaml filename for map_server (#3174)

* implement launch priority for the mapserver parameter yaml_filename

minor fix

fix commit

reincluded rewritten function

comment remaining lines for yaml_filename

removed default_value

issue with composable node

alternative soltion to the condition param not working in composable node

remove unused import

remove comments and reorder composablenode execution

fixing commit

fixing format

fixing lint

Update nav2_bringup/params/nav2_params.yaml

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

* state new ros-rolling release changes and deprecation

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

* adding reconfigure test to thetastar (#3275)

* Check for range_max of laserscan in updateFilter to avoid a implicit overflow crash. (#3276)

* Update amcl_node.cpp

* fit the code style

* fit code style

* BT Service Node to throw if service was not available in time (#3256)

* throw if service server wasn't available in time

mimic the behavior of the bt action node constructor

* throw if action unavailable in bt cancel action

* use chrono literals namespace

* fix linting errors

* fix code style divergence

* remove exec_depend on behaviortree_cpp_v3 (#3279)

* add parameterized refinement recursion numbers in Smac Planner Smoother and Simple Smoother (#3284)

* add parameterized refinement recursion numbers

* fix tests

* Add allow_unknown parameter to theta star planner (#3286)

* Add allow unknown parameter to theta star planner

* Add allow unknown parameter to tests

* missing comma

* Change cost of unknown tiles

* Uncrustify

* Include test cases waypoint follwer (#3288)

* WIP

* included missed_waypoing check

* finished inclding test

* fix format

* return default sleep value

* Dynamically changing polygons support (#3245)

* Add Collision Monitor polygon topics subscription

* Add the support of polygons published in different frame

* Internal review

* Fix working with polygons visualization

* Update nav2_collision_monitor/README.md

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

* Move getTransform to nav2_util

* Fix misprint

* Meet remaining review items:
* Update polygon params handling logic
* Warn if polygon shape was not set
* Publish with ownership movement

* Correct polygons_test.cpp parameters handling logic

* Adjust README for dynamic polygons logic update

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

* adding getCostScalingFactor (#3290)

* Implemented smoother selector bt node (#3283)

* Implemented smoother selector bt node

Signed-off-by: Owen Hooper <17ofh@queensu.ca>

* updated copyright in modified file

Signed-off-by: Owen Hooper <17ofh@queensu.ca>

Signed-off-by: Owen Hooper <17ofh@queensu.ca>

* Pipe error codes (#3251)

* issue with finding key

* passed up codes to bt_navigator

* lint fix

* updates

* adding error_code_id back in

* error codes names in params

* bump error codes

* lint

* spelling

* test fix

* update behavior trees

* cleanup

* Update bt_action_server_impl.hpp

* code review

* lint

* code review

* log fix

* error code for waypoint follower

* clean up

* remove waypoint error test, too flaky on CI

* lint and code review

* rough imp for waypoint changes

* lint

* code review

* build fix

* clean up

* revert

* space

* remove

* try to make github happ

* stop gap

* loading in param file

* working tests :)

* lint

* fixed cmake

* lint

* lint

* trigger build

* added invalid plugin error

* added test for piping up error codes

* clean up

* test waypoint follower

* only launch what is needed

* waypoint test

* revert lines for robot navigator

* fix test

* waypoint test

* switched to uint16

* clean up

* code review

* todo to note

* lint

* remove comment

* Update nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server_impl.hpp

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

* rename error_codes

* error code for navigate to pose

* error codes for navigate through poses.

* error codes for navigate through poses

* message update for waypoint follower

* rename to error code

* update node xml

Co-authored-by: Joshua Wallace <josho.wallace.com>
Co-authored-by: Steve Macenski <stevenmacenski@gmail.com>

* Solve bug when CostmapInfoServer is reactivated (#3292)

* Solve bug when CostmapInfoServer is reactivated

* Smoothness metrics update (#3294)

* Update metrics for path smoothness

* Support Savitzky-Golay smoother

* preempt/cancel test for time behavior, spin pluguin (#3301)

* include preempt/cancel test for time behavior, spin pluguin

* linting

* fix bug in code

* lint fix

* clean up test

* lint

* cleaned up test

* update

* revert Cmake

* fixed error code test

* half time

* bump build

* revert

* lint error

Signed-off-by: Owen Hooper <17ofh@queensu.ca>
Co-authored-by: Joshua Wallace <josho.wallace.com>
Co-authored-by: Hao-Xuan Song <44140526+Cryst4L9527@users.noreply.github.com>
Co-authored-by: jaeminSHIN <91681721+woawo1213@users.noreply.github.com>
Co-authored-by: Steve Macenski <stevenmacenski@gmail.com>
Co-authored-by: Afif Swaidan <53655365+afifswaidan@users.noreply.github.com>
Co-authored-by: Afif Swaidan <afif.swaidan@spexal.com>
Co-authored-by: Stevedan Ogochukwu Omodolor <61468301+stevedanomodolor@users.noreply.github.com>
Co-authored-by: Pradheep Krishna <padhupradheep@gmail.com>
Co-authored-by: Erwin Lejeune <erwin.lejeune15@gmail.com>
Co-authored-by: Adam Aposhian <aposhian.dev@gmail.com>
Co-authored-by: Pedro Alejandro González <71234974+pepisg@users.noreply.github.com>
Co-authored-by: Alexey Merzlyakov <60094858+AlexeyMerzlyakov@users.noreply.github.com>
Co-authored-by: Owen Hooper <17ofh@queensu.ca>
Co-authored-by: MartiBolet <43337758+MartiBolet@users.noreply.github.com>
jwallace42 pushed a commit to jwallace42/navigation2 that referenced this pull request Dec 14, 2022
…os-navigation#3174)

* implement launch priority for the mapserver parameter yaml_filename

minor fix

fix commit

reincluded rewritten function

comment remaining lines for yaml_filename

removed default_value

issue with composable node

alternative soltion to the condition param not working in composable node

remove unused import

remove comments and reorder composablenode execution

fixing commit

fixing format

fixing lint

Update nav2_bringup/params/nav2_params.yaml

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

* state new ros-rolling release changes and deprecation

Co-authored-by: Steve Macenski <stevenmacenski@gmail.com>
jwallace42 added a commit to jwallace42/navigation2 that referenced this pull request Dec 14, 2022
* publish layers of costmap

* lint fix

* lint round 2 :)

* code review

* remove isPublishable

* lint

* test running

* rough structure complete

* completed test

* lint

* code review

* CI

* CI

* linting

* completed pub test

* CostmapLayer::matchSize may be executed concurrently (ros-navigation#3250)

* CostmapLayer::matchSize() add a mutex

* Fix typo (ros-navigation#3262)

* Adding new Nav2 Smoother: Savitzky-Golay Smoother (ros-navigation#3264)

* initial prototype of the Savitzky Golay Filter Path Smoother

* fixed indexing issue - tested working

* updates for filter

* adding unit tests for SG-filter smoother

* adding lifecycle transitions

* refactoring RPP a bit for cleanliness on way to ROSCon (ros-navigation#3265)

* refactor for RPP on way to ROSCon

* fixing header

* fixing header

* fixing header

* fix edge cases test samplings

* linting

* exceptions for compute path through poses (ros-navigation#3248)

* exceptions for compute path through poses

* lint fix

* code review

* code review

Co-authored-by: Joshua Wallace <josho.wallace.com>

* Reclaim Our CI Coverage from the Lords of Painful Subtle Regressions ⚔️⚔️⚔️ (ros-navigation#3266)

* test waypoint follower with composition off for logging

* adding no composition to all system tests

* Added Line Iterator (ros-navigation#3197)

* Added Line Iterator

* Updated Line Iterator to a new iteration method

* Added the resolution as a parameter/ fixed linting

* Added the resolution as a parameter/ fixed linting

* Added unittests for the line iterator

* Added unittests based on "unittest" package

* Fixed __init__.py and rephrased some docstrings

* Fixed linting errors

* Fixed Linting Errors

* Added some unittests and removed some methods

* Dummy commit for CircleCI Issue

Co-authored-by: Afif Swaidan <afif.swaidan@spexal.com>

* Use SetParameter Launch API to set the yaml filename for map_server (ros-navigation#3174)

* implement launch priority for the mapserver parameter yaml_filename

minor fix

fix commit

reincluded rewritten function

comment remaining lines for yaml_filename

removed default_value

issue with composable node

alternative soltion to the condition param not working in composable node

remove unused import

remove comments and reorder composablenode execution

fixing commit

fixing format

fixing lint

Update nav2_bringup/params/nav2_params.yaml

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

* state new ros-rolling release changes and deprecation

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

* adding reconfigure test to thetastar (ros-navigation#3275)

* Check for range_max of laserscan in updateFilter to avoid a implicit overflow crash. (ros-navigation#3276)

* Update amcl_node.cpp

* fit the code style

* fit code style

* BT Service Node to throw if service was not available in time (ros-navigation#3256)

* throw if service server wasn't available in time

mimic the behavior of the bt action node constructor

* throw if action unavailable in bt cancel action

* use chrono literals namespace

* fix linting errors

* fix code style divergence

* remove exec_depend on behaviortree_cpp_v3 (ros-navigation#3279)

* add parameterized refinement recursion numbers in Smac Planner Smoother and Simple Smoother (ros-navigation#3284)

* add parameterized refinement recursion numbers

* fix tests

* Add allow_unknown parameter to theta star planner (ros-navigation#3286)

* Add allow unknown parameter to theta star planner

* Add allow unknown parameter to tests

* missing comma

* Change cost of unknown tiles

* Uncrustify

* Include test cases waypoint follwer (ros-navigation#3288)

* WIP

* included missed_waypoing check

* finished inclding test

* fix format

* return default sleep value

* Dynamically changing polygons support (ros-navigation#3245)

* Add Collision Monitor polygon topics subscription

* Add the support of polygons published in different frame

* Internal review

* Fix working with polygons visualization

* Update nav2_collision_monitor/README.md

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

* Move getTransform to nav2_util

* Fix misprint

* Meet remaining review items:
* Update polygon params handling logic
* Warn if polygon shape was not set
* Publish with ownership movement

* Correct polygons_test.cpp parameters handling logic

* Adjust README for dynamic polygons logic update

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

* adding getCostScalingFactor (ros-navigation#3290)

* Implemented smoother selector bt node (ros-navigation#3283)

* Implemented smoother selector bt node

Signed-off-by: Owen Hooper <17ofh@queensu.ca>

* updated copyright in modified file

Signed-off-by: Owen Hooper <17ofh@queensu.ca>

Signed-off-by: Owen Hooper <17ofh@queensu.ca>

* Pipe error codes (ros-navigation#3251)

* issue with finding key

* passed up codes to bt_navigator

* lint fix

* updates

* adding error_code_id back in

* error codes names in params

* bump error codes

* lint

* spelling

* test fix

* update behavior trees

* cleanup

* Update bt_action_server_impl.hpp

* code review

* lint

* code review

* log fix

* error code for waypoint follower

* clean up

* remove waypoint error test, too flaky on CI

* lint and code review

* rough imp for waypoint changes

* lint

* code review

* build fix

* clean up

* revert

* space

* remove

* try to make github happ

* stop gap

* loading in param file

* working tests :)

* lint

* fixed cmake

* lint

* lint

* trigger build

* added invalid plugin error

* added test for piping up error codes

* clean up

* test waypoint follower

* only launch what is needed

* waypoint test

* revert lines for robot navigator

* fix test

* waypoint test

* switched to uint16

* clean up

* code review

* todo to note

* lint

* remove comment

* Update nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server_impl.hpp

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

* rename error_codes

* error code for navigate to pose

* error codes for navigate through poses.

* error codes for navigate through poses

* message update for waypoint follower

* rename to error code

* update node xml

Co-authored-by: Joshua Wallace <josho.wallace.com>
Co-authored-by: Steve Macenski <stevenmacenski@gmail.com>

* Solve bug when CostmapInfoServer is reactivated (ros-navigation#3292)

* Solve bug when CostmapInfoServer is reactivated

* Smoothness metrics update (ros-navigation#3294)

* Update metrics for path smoothness

* Support Savitzky-Golay smoother

* preempt/cancel test for time behavior, spin pluguin (ros-navigation#3301)

* include preempt/cancel test for time behavior, spin pluguin

* linting

* fix bug in code

* lint fix

* clean up test

* lint

* cleaned up test

* update

* revert Cmake

Signed-off-by: Owen Hooper <17ofh@queensu.ca>
Co-authored-by: Joshua Wallace <josho.wallace.com>
Co-authored-by: Hao-Xuan Song <44140526+Cryst4L9527@users.noreply.github.com>
Co-authored-by: jaeminSHIN <91681721+woawo1213@users.noreply.github.com>
Co-authored-by: Steve Macenski <stevenmacenski@gmail.com>
Co-authored-by: Afif Swaidan <53655365+afifswaidan@users.noreply.github.com>
Co-authored-by: Afif Swaidan <afif.swaidan@spexal.com>
Co-authored-by: Stevedan Ogochukwu Omodolor <61468301+stevedanomodolor@users.noreply.github.com>
Co-authored-by: Pradheep Krishna <padhupradheep@gmail.com>
Co-authored-by: Erwin Lejeune <erwin.lejeune15@gmail.com>
Co-authored-by: Adam Aposhian <aposhian.dev@gmail.com>
Co-authored-by: Pedro Alejandro González <71234974+pepisg@users.noreply.github.com>
Co-authored-by: Alexey Merzlyakov <60094858+AlexeyMerzlyakov@users.noreply.github.com>
Co-authored-by: Owen Hooper <17ofh@queensu.ca>
Co-authored-by: MartiBolet <43337758+MartiBolet@users.noreply.github.com>
jwallace42 added a commit to jwallace42/navigation2 that referenced this pull request Dec 14, 2022
* publish layers of costmap

* lint fix

* lint round 2 :)

* code review

* remove isPublishable

* lint

* test running

* rough structure complete

* completed test

* lint

* code review

* CI

* CI

* linting

* completed pub test

* CostmapLayer::matchSize may be executed concurrently (ros-navigation#3250)

* CostmapLayer::matchSize() add a mutex

* Fix typo (ros-navigation#3262)

* Adding new Nav2 Smoother: Savitzky-Golay Smoother (ros-navigation#3264)

* initial prototype of the Savitzky Golay Filter Path Smoother

* fixed indexing issue - tested working

* updates for filter

* adding unit tests for SG-filter smoother

* adding lifecycle transitions

* refactoring RPP a bit for cleanliness on way to ROSCon (ros-navigation#3265)

* refactor for RPP on way to ROSCon

* fixing header

* fixing header

* fixing header

* fix edge cases test samplings

* linting

* exceptions for compute path through poses (ros-navigation#3248)

* exceptions for compute path through poses

* lint fix

* code review

* code review

Co-authored-by: Joshua Wallace <josho.wallace.com>

* Reclaim Our CI Coverage from the Lords of Painful Subtle Regressions ⚔️⚔️⚔️ (ros-navigation#3266)

* test waypoint follower with composition off for logging

* adding no composition to all system tests

* Added Line Iterator (ros-navigation#3197)

* Added Line Iterator

* Updated Line Iterator to a new iteration method

* Added the resolution as a parameter/ fixed linting

* Added the resolution as a parameter/ fixed linting

* Added unittests for the line iterator

* Added unittests based on "unittest" package

* Fixed __init__.py and rephrased some docstrings

* Fixed linting errors

* Fixed Linting Errors

* Added some unittests and removed some methods

* Dummy commit for CircleCI Issue

Co-authored-by: Afif Swaidan <afif.swaidan@spexal.com>

* Use SetParameter Launch API to set the yaml filename for map_server (ros-navigation#3174)

* implement launch priority for the mapserver parameter yaml_filename

minor fix

fix commit

reincluded rewritten function

comment remaining lines for yaml_filename

removed default_value

issue with composable node

alternative soltion to the condition param not working in composable node

remove unused import

remove comments and reorder composablenode execution

fixing commit

fixing format

fixing lint

Update nav2_bringup/params/nav2_params.yaml

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

* state new ros-rolling release changes and deprecation

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

* adding reconfigure test to thetastar (ros-navigation#3275)

* Check for range_max of laserscan in updateFilter to avoid a implicit overflow crash. (ros-navigation#3276)

* Update amcl_node.cpp

* fit the code style

* fit code style

* BT Service Node to throw if service was not available in time (ros-navigation#3256)

* throw if service server wasn't available in time

mimic the behavior of the bt action node constructor

* throw if action unavailable in bt cancel action

* use chrono literals namespace

* fix linting errors

* fix code style divergence

* remove exec_depend on behaviortree_cpp_v3 (ros-navigation#3279)

* add parameterized refinement recursion numbers in Smac Planner Smoother and Simple Smoother (ros-navigation#3284)

* add parameterized refinement recursion numbers

* fix tests

* Add allow_unknown parameter to theta star planner (ros-navigation#3286)

* Add allow unknown parameter to theta star planner

* Add allow unknown parameter to tests

* missing comma

* Change cost of unknown tiles

* Uncrustify

* Include test cases waypoint follwer (ros-navigation#3288)

* WIP

* included missed_waypoing check

* finished inclding test

* fix format

* return default sleep value

* Dynamically changing polygons support (ros-navigation#3245)

* Add Collision Monitor polygon topics subscription

* Add the support of polygons published in different frame

* Internal review

* Fix working with polygons visualization

* Update nav2_collision_monitor/README.md

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

* Move getTransform to nav2_util

* Fix misprint

* Meet remaining review items:
* Update polygon params handling logic
* Warn if polygon shape was not set
* Publish with ownership movement

* Correct polygons_test.cpp parameters handling logic

* Adjust README for dynamic polygons logic update

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

* adding getCostScalingFactor (ros-navigation#3290)

* Implemented smoother selector bt node (ros-navigation#3283)

* Implemented smoother selector bt node

Signed-off-by: Owen Hooper <17ofh@queensu.ca>

* updated copyright in modified file

Signed-off-by: Owen Hooper <17ofh@queensu.ca>

Signed-off-by: Owen Hooper <17ofh@queensu.ca>

* Pipe error codes (ros-navigation#3251)

* issue with finding key

* passed up codes to bt_navigator

* lint fix

* updates

* adding error_code_id back in

* error codes names in params

* bump error codes

* lint

* spelling

* test fix

* update behavior trees

* cleanup

* Update bt_action_server_impl.hpp

* code review

* lint

* code review

* log fix

* error code for waypoint follower

* clean up

* remove waypoint error test, too flaky on CI

* lint and code review

* rough imp for waypoint changes

* lint

* code review

* build fix

* clean up

* revert

* space

* remove

* try to make github happ

* stop gap

* loading in param file

* working tests :)

* lint

* fixed cmake

* lint

* lint

* trigger build

* added invalid plugin error

* added test for piping up error codes

* clean up

* test waypoint follower

* only launch what is needed

* waypoint test

* revert lines for robot navigator

* fix test

* waypoint test

* switched to uint16

* clean up

* code review

* todo to note

* lint

* remove comment

* Update nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server_impl.hpp

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

* rename error_codes

* error code for navigate to pose

* error codes for navigate through poses.

* error codes for navigate through poses

* message update for waypoint follower

* rename to error code

* update node xml

Co-authored-by: Joshua Wallace <josho.wallace.com>
Co-authored-by: Steve Macenski <stevenmacenski@gmail.com>

* Solve bug when CostmapInfoServer is reactivated (ros-navigation#3292)

* Solve bug when CostmapInfoServer is reactivated

* Smoothness metrics update (ros-navigation#3294)

* Update metrics for path smoothness

* Support Savitzky-Golay smoother

* preempt/cancel test for time behavior, spin pluguin (ros-navigation#3301)

* include preempt/cancel test for time behavior, spin pluguin

* linting

* fix bug in code

* lint fix

* clean up test

* lint

* cleaned up test

* update

* revert Cmake

* fixed error code test

* half time

* bump build

* revert

* lint error

Signed-off-by: Owen Hooper <17ofh@queensu.ca>
Co-authored-by: Joshua Wallace <josho.wallace.com>
Co-authored-by: Hao-Xuan Song <44140526+Cryst4L9527@users.noreply.github.com>
Co-authored-by: jaeminSHIN <91681721+woawo1213@users.noreply.github.com>
Co-authored-by: Steve Macenski <stevenmacenski@gmail.com>
Co-authored-by: Afif Swaidan <53655365+afifswaidan@users.noreply.github.com>
Co-authored-by: Afif Swaidan <afif.swaidan@spexal.com>
Co-authored-by: Stevedan Ogochukwu Omodolor <61468301+stevedanomodolor@users.noreply.github.com>
Co-authored-by: Pradheep Krishna <padhupradheep@gmail.com>
Co-authored-by: Erwin Lejeune <erwin.lejeune15@gmail.com>
Co-authored-by: Adam Aposhian <aposhian.dev@gmail.com>
Co-authored-by: Pedro Alejandro González <71234974+pepisg@users.noreply.github.com>
Co-authored-by: Alexey Merzlyakov <60094858+AlexeyMerzlyakov@users.noreply.github.com>
Co-authored-by: Owen Hooper <17ofh@queensu.ca>
Co-authored-by: MartiBolet <43337758+MartiBolet@users.noreply.github.com>
andrewlycas pushed a commit to StratomInc/navigation2 that referenced this pull request Feb 23, 2023
* publish layers of costmap

* lint fix

* lint round 2 :)

* code review

* remove isPublishable

* lint

* test running

* rough structure complete

* completed test

* lint

* code review

* CI

* CI

* linting

* completed pub test

* CostmapLayer::matchSize may be executed concurrently (ros-navigation#3250)

* CostmapLayer::matchSize() add a mutex

* Fix typo (ros-navigation#3262)

* Adding new Nav2 Smoother: Savitzky-Golay Smoother (ros-navigation#3264)

* initial prototype of the Savitzky Golay Filter Path Smoother

* fixed indexing issue - tested working

* updates for filter

* adding unit tests for SG-filter smoother

* adding lifecycle transitions

* refactoring RPP a bit for cleanliness on way to ROSCon (ros-navigation#3265)

* refactor for RPP on way to ROSCon

* fixing header

* fixing header

* fixing header

* fix edge cases test samplings

* linting

* exceptions for compute path through poses (ros-navigation#3248)

* exceptions for compute path through poses

* lint fix

* code review

* code review

Co-authored-by: Joshua Wallace <josho.wallace.com>

* Reclaim Our CI Coverage from the Lords of Painful Subtle Regressions ⚔️⚔️⚔️ (ros-navigation#3266)

* test waypoint follower with composition off for logging

* adding no composition to all system tests

* Added Line Iterator (ros-navigation#3197)

* Added Line Iterator

* Updated Line Iterator to a new iteration method

* Added the resolution as a parameter/ fixed linting

* Added the resolution as a parameter/ fixed linting

* Added unittests for the line iterator

* Added unittests based on "unittest" package

* Fixed __init__.py and rephrased some docstrings

* Fixed linting errors

* Fixed Linting Errors

* Added some unittests and removed some methods

* Dummy commit for CircleCI Issue

Co-authored-by: Afif Swaidan <afif.swaidan@spexal.com>

* Use SetParameter Launch API to set the yaml filename for map_server (ros-navigation#3174)

* implement launch priority for the mapserver parameter yaml_filename

minor fix

fix commit

reincluded rewritten function

comment remaining lines for yaml_filename

removed default_value

issue with composable node

alternative soltion to the condition param not working in composable node

remove unused import

remove comments and reorder composablenode execution

fixing commit

fixing format

fixing lint

Update nav2_bringup/params/nav2_params.yaml

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

* state new ros-rolling release changes and deprecation

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

* adding reconfigure test to thetastar (ros-navigation#3275)

* Check for range_max of laserscan in updateFilter to avoid a implicit overflow crash. (ros-navigation#3276)

* Update amcl_node.cpp

* fit the code style

* fit code style

* BT Service Node to throw if service was not available in time (ros-navigation#3256)

* throw if service server wasn't available in time

mimic the behavior of the bt action node constructor

* throw if action unavailable in bt cancel action

* use chrono literals namespace

* fix linting errors

* fix code style divergence

* remove exec_depend on behaviortree_cpp_v3 (ros-navigation#3279)

* add parameterized refinement recursion numbers in Smac Planner Smoother and Simple Smoother (ros-navigation#3284)

* add parameterized refinement recursion numbers

* fix tests

* Add allow_unknown parameter to theta star planner (ros-navigation#3286)

* Add allow unknown parameter to theta star planner

* Add allow unknown parameter to tests

* missing comma

* Change cost of unknown tiles

* Uncrustify

* Include test cases waypoint follwer (ros-navigation#3288)

* WIP

* included missed_waypoing check

* finished inclding test

* fix format

* return default sleep value

* Dynamically changing polygons support (ros-navigation#3245)

* Add Collision Monitor polygon topics subscription

* Add the support of polygons published in different frame

* Internal review

* Fix working with polygons visualization

* Update nav2_collision_monitor/README.md

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

* Move getTransform to nav2_util

* Fix misprint

* Meet remaining review items:
* Update polygon params handling logic
* Warn if polygon shape was not set
* Publish with ownership movement

* Correct polygons_test.cpp parameters handling logic

* Adjust README for dynamic polygons logic update

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

* adding getCostScalingFactor (ros-navigation#3290)

* Implemented smoother selector bt node (ros-navigation#3283)

* Implemented smoother selector bt node

Signed-off-by: Owen Hooper <17ofh@queensu.ca>

* updated copyright in modified file

Signed-off-by: Owen Hooper <17ofh@queensu.ca>

Signed-off-by: Owen Hooper <17ofh@queensu.ca>

* Pipe error codes (ros-navigation#3251)

* issue with finding key

* passed up codes to bt_navigator

* lint fix

* updates

* adding error_code_id back in

* error codes names in params

* bump error codes

* lint

* spelling

* test fix

* update behavior trees

* cleanup

* Update bt_action_server_impl.hpp

* code review

* lint

* code review

* log fix

* error code for waypoint follower

* clean up

* remove waypoint error test, too flaky on CI

* lint and code review

* rough imp for waypoint changes

* lint

* code review

* build fix

* clean up

* revert

* space

* remove

* try to make github happ

* stop gap

* loading in param file

* working tests :)

* lint

* fixed cmake

* lint

* lint

* trigger build

* added invalid plugin error

* added test for piping up error codes

* clean up

* test waypoint follower

* only launch what is needed

* waypoint test

* revert lines for robot navigator

* fix test

* waypoint test

* switched to uint16

* clean up

* code review

* todo to note

* lint

* remove comment

* Update nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server_impl.hpp

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

* rename error_codes

* error code for navigate to pose

* error codes for navigate through poses.

* error codes for navigate through poses

* message update for waypoint follower

* rename to error code

* update node xml

Co-authored-by: Joshua Wallace <josho.wallace.com>
Co-authored-by: Steve Macenski <stevenmacenski@gmail.com>

* Solve bug when CostmapInfoServer is reactivated (ros-navigation#3292)

* Solve bug when CostmapInfoServer is reactivated

* Smoothness metrics update (ros-navigation#3294)

* Update metrics for path smoothness

* Support Savitzky-Golay smoother

* preempt/cancel test for time behavior, spin pluguin (ros-navigation#3301)

* include preempt/cancel test for time behavior, spin pluguin

* linting

* fix bug in code

* lint fix

* clean up test

* lint

* cleaned up test

* update

* revert Cmake

Signed-off-by: Owen Hooper <17ofh@queensu.ca>
Co-authored-by: Joshua Wallace <josho.wallace.com>
Co-authored-by: Hao-Xuan Song <44140526+Cryst4L9527@users.noreply.github.com>
Co-authored-by: jaeminSHIN <91681721+woawo1213@users.noreply.github.com>
Co-authored-by: Steve Macenski <stevenmacenski@gmail.com>
Co-authored-by: Afif Swaidan <53655365+afifswaidan@users.noreply.github.com>
Co-authored-by: Afif Swaidan <afif.swaidan@spexal.com>
Co-authored-by: Stevedan Ogochukwu Omodolor <61468301+stevedanomodolor@users.noreply.github.com>
Co-authored-by: Pradheep Krishna <padhupradheep@gmail.com>
Co-authored-by: Erwin Lejeune <erwin.lejeune15@gmail.com>
Co-authored-by: Adam Aposhian <aposhian.dev@gmail.com>
Co-authored-by: Pedro Alejandro González <71234974+pepisg@users.noreply.github.com>
Co-authored-by: Alexey Merzlyakov <60094858+AlexeyMerzlyakov@users.noreply.github.com>
Co-authored-by: Owen Hooper <17ofh@queensu.ca>
Co-authored-by: MartiBolet <43337758+MartiBolet@users.noreply.github.com>
andrewlycas pushed a commit to StratomInc/navigation2 that referenced this pull request Feb 23, 2023
* publish layers of costmap

* lint fix

* lint round 2 :)

* code review

* remove isPublishable

* lint

* test running

* rough structure complete

* completed test

* lint

* code review

* CI

* CI

* linting

* completed pub test

* CostmapLayer::matchSize may be executed concurrently (ros-navigation#3250)

* CostmapLayer::matchSize() add a mutex

* Fix typo (ros-navigation#3262)

* Adding new Nav2 Smoother: Savitzky-Golay Smoother (ros-navigation#3264)

* initial prototype of the Savitzky Golay Filter Path Smoother

* fixed indexing issue - tested working

* updates for filter

* adding unit tests for SG-filter smoother

* adding lifecycle transitions

* refactoring RPP a bit for cleanliness on way to ROSCon (ros-navigation#3265)

* refactor for RPP on way to ROSCon

* fixing header

* fixing header

* fixing header

* fix edge cases test samplings

* linting

* exceptions for compute path through poses (ros-navigation#3248)

* exceptions for compute path through poses

* lint fix

* code review

* code review

Co-authored-by: Joshua Wallace <josho.wallace.com>

* Reclaim Our CI Coverage from the Lords of Painful Subtle Regressions ⚔️⚔️⚔️ (ros-navigation#3266)

* test waypoint follower with composition off for logging

* adding no composition to all system tests

* Added Line Iterator (ros-navigation#3197)

* Added Line Iterator

* Updated Line Iterator to a new iteration method

* Added the resolution as a parameter/ fixed linting

* Added the resolution as a parameter/ fixed linting

* Added unittests for the line iterator

* Added unittests based on "unittest" package

* Fixed __init__.py and rephrased some docstrings

* Fixed linting errors

* Fixed Linting Errors

* Added some unittests and removed some methods

* Dummy commit for CircleCI Issue

Co-authored-by: Afif Swaidan <afif.swaidan@spexal.com>

* Use SetParameter Launch API to set the yaml filename for map_server (ros-navigation#3174)

* implement launch priority for the mapserver parameter yaml_filename

minor fix

fix commit

reincluded rewritten function

comment remaining lines for yaml_filename

removed default_value

issue with composable node

alternative soltion to the condition param not working in composable node

remove unused import

remove comments and reorder composablenode execution

fixing commit

fixing format

fixing lint

Update nav2_bringup/params/nav2_params.yaml

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

* state new ros-rolling release changes and deprecation

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

* adding reconfigure test to thetastar (ros-navigation#3275)

* Check for range_max of laserscan in updateFilter to avoid a implicit overflow crash. (ros-navigation#3276)

* Update amcl_node.cpp

* fit the code style

* fit code style

* BT Service Node to throw if service was not available in time (ros-navigation#3256)

* throw if service server wasn't available in time

mimic the behavior of the bt action node constructor

* throw if action unavailable in bt cancel action

* use chrono literals namespace

* fix linting errors

* fix code style divergence

* remove exec_depend on behaviortree_cpp_v3 (ros-navigation#3279)

* add parameterized refinement recursion numbers in Smac Planner Smoother and Simple Smoother (ros-navigation#3284)

* add parameterized refinement recursion numbers

* fix tests

* Add allow_unknown parameter to theta star planner (ros-navigation#3286)

* Add allow unknown parameter to theta star planner

* Add allow unknown parameter to tests

* missing comma

* Change cost of unknown tiles

* Uncrustify

* Include test cases waypoint follwer (ros-navigation#3288)

* WIP

* included missed_waypoing check

* finished inclding test

* fix format

* return default sleep value

* Dynamically changing polygons support (ros-navigation#3245)

* Add Collision Monitor polygon topics subscription

* Add the support of polygons published in different frame

* Internal review

* Fix working with polygons visualization

* Update nav2_collision_monitor/README.md

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

* Move getTransform to nav2_util

* Fix misprint

* Meet remaining review items:
* Update polygon params handling logic
* Warn if polygon shape was not set
* Publish with ownership movement

* Correct polygons_test.cpp parameters handling logic

* Adjust README for dynamic polygons logic update

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

* adding getCostScalingFactor (ros-navigation#3290)

* Implemented smoother selector bt node (ros-navigation#3283)

* Implemented smoother selector bt node

Signed-off-by: Owen Hooper <17ofh@queensu.ca>

* updated copyright in modified file

Signed-off-by: Owen Hooper <17ofh@queensu.ca>

Signed-off-by: Owen Hooper <17ofh@queensu.ca>

* Pipe error codes (ros-navigation#3251)

* issue with finding key

* passed up codes to bt_navigator

* lint fix

* updates

* adding error_code_id back in

* error codes names in params

* bump error codes

* lint

* spelling

* test fix

* update behavior trees

* cleanup

* Update bt_action_server_impl.hpp

* code review

* lint

* code review

* log fix

* error code for waypoint follower

* clean up

* remove waypoint error test, too flaky on CI

* lint and code review

* rough imp for waypoint changes

* lint

* code review

* build fix

* clean up

* revert

* space

* remove

* try to make github happ

* stop gap

* loading in param file

* working tests :)

* lint

* fixed cmake

* lint

* lint

* trigger build

* added invalid plugin error

* added test for piping up error codes

* clean up

* test waypoint follower

* only launch what is needed

* waypoint test

* revert lines for robot navigator

* fix test

* waypoint test

* switched to uint16

* clean up

* code review

* todo to note

* lint

* remove comment

* Update nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server_impl.hpp

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

* rename error_codes

* error code for navigate to pose

* error codes for navigate through poses.

* error codes for navigate through poses

* message update for waypoint follower

* rename to error code

* update node xml

Co-authored-by: Joshua Wallace <josho.wallace.com>
Co-authored-by: Steve Macenski <stevenmacenski@gmail.com>

* Solve bug when CostmapInfoServer is reactivated (ros-navigation#3292)

* Solve bug when CostmapInfoServer is reactivated

* Smoothness metrics update (ros-navigation#3294)

* Update metrics for path smoothness

* Support Savitzky-Golay smoother

* preempt/cancel test for time behavior, spin pluguin (ros-navigation#3301)

* include preempt/cancel test for time behavior, spin pluguin

* linting

* fix bug in code

* lint fix

* clean up test

* lint

* cleaned up test

* update

* revert Cmake

* fixed error code test

* half time

* bump build

* revert

* lint error

Signed-off-by: Owen Hooper <17ofh@queensu.ca>
Co-authored-by: Joshua Wallace <josho.wallace.com>
Co-authored-by: Hao-Xuan Song <44140526+Cryst4L9527@users.noreply.github.com>
Co-authored-by: jaeminSHIN <91681721+woawo1213@users.noreply.github.com>
Co-authored-by: Steve Macenski <stevenmacenski@gmail.com>
Co-authored-by: Afif Swaidan <53655365+afifswaidan@users.noreply.github.com>
Co-authored-by: Afif Swaidan <afif.swaidan@spexal.com>
Co-authored-by: Stevedan Ogochukwu Omodolor <61468301+stevedanomodolor@users.noreply.github.com>
Co-authored-by: Pradheep Krishna <padhupradheep@gmail.com>
Co-authored-by: Erwin Lejeune <erwin.lejeune15@gmail.com>
Co-authored-by: Adam Aposhian <aposhian.dev@gmail.com>
Co-authored-by: Pedro Alejandro González <71234974+pepisg@users.noreply.github.com>
Co-authored-by: Alexey Merzlyakov <60094858+AlexeyMerzlyakov@users.noreply.github.com>
Co-authored-by: Owen Hooper <17ofh@queensu.ca>
Co-authored-by: MartiBolet <43337758+MartiBolet@users.noreply.github.com>
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.

None yet

4 participants