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

Is this a code bug? #4268

Closed
jeezrick opened this issue Apr 18, 2024 · 4 comments
Closed

Is this a code bug? #4268

jeezrick opened this issue Apr 18, 2024 · 4 comments

Comments

@jeezrick
Copy link

bool GridCollisionChecker::inCollision(
  const float & x,
  const float & y,
  const float & angle_bin,
  const bool & traverse_unknown)
{
  // Check to make sure cell is inside the map
  if (outsideRange(costmap_->getSizeInCellsX(), x) ||
    outsideRange(costmap_->getSizeInCellsY(), y))
  {
    return false;
  }



...

bool NodeHybrid::isNodeValid(
  const bool & traverse_unknown,
  GridCollisionChecker * collision_checker)
{
  if (collision_checker->inCollision(
      this->pose.x, this->pose.y, this->pose.theta /*bin number*/, traverse_unknown))
  {
    return false;
  }

  _cell_cost = collision_checker->getCost();
  return true;
}

Isn't this should return true? When node x or y is outside range, this node is invalid, hence inCollision function should return true, then isNodeValid function should return false.

@SteveMacenski
Copy link
Member

@jwallace42 that's part of your PR b232341 -- thoughts?

I would tend to agree, but I want to make sure the original author agrees as well and there isn't something subtle he's relying on with that condition. @jeezrick can you open up a PR to invert that? Should be easy for me to merge if he's on the same page.

@jwallace42
Copy link
Contributor

Yeah,

that should be flipped.

@SteveMacenski
Copy link
Member

@jeezrick please submit a PR and I can merge! Thanks for pointing this out!

SteveMacenski added a commit that referenced this issue May 3, 2024
Signed-off-by: Steve Macenski <stevenmacenski@gmail.com>
@SteveMacenski
Copy link
Member

Its been a few weeks, so I just took care of it :-) https://github.com/open-navigation/navigation2/pull/4296

Thanks for the note @jeezrick !

SteveMacenski added a commit that referenced this issue May 3, 2024
Signed-off-by: Steve Macenski <stevenmacenski@gmail.com>
enricosutera pushed a commit to enricosutera/navigation2 that referenced this issue May 19, 2024
Signed-off-by: Steve Macenski <stevenmacenski@gmail.com>
Signed-off-by: enricosutera <enricosutera@outlook.com>
SteveMacenski added a commit that referenced this issue May 23, 2024
Signed-off-by: Steve Macenski <stevenmacenski@gmail.com>
SteveMacenski added a commit that referenced this issue May 23, 2024
Signed-off-by: Steve Macenski <stevenmacenski@gmail.com>
SteveMacenski added a commit that referenced this issue May 23, 2024
* Scale cost critic's weight when dynamically updated (#4246)

* Scale cost critic's weight when dynamically updated

Signed-off-by: pepisg <pedro.gonzalez@eia.edu.co>

* sign off

Signed-off-by: pepisg <pedro.gonzalez@eia.edu.co>

---------

Signed-off-by: pepisg <pedro.gonzalez@eia.edu.co>

* Add expanding the ~/ to the full home dir of user in the path to the map yaml.  (#4258)

* Add user home expander of home sequence

Signed-off-by: Wiktor Bajor <wiktorbajor1@gmail.com>

* Add passing home dir as string instead of const char*

Signed-off-by: Wiktor Bajor <wiktorbajor1@gmail.com>

* Add docs

Signed-off-by: Wiktor Bajor <wiktorbajor1@gmail.com>

* Fix function declaration

Signed-off-by: Wiktor Bajor <wiktorbajor1@gmail.com>

* Fix linter issues

Signed-off-by: Wiktor Bajor <wiktorbajor1@gmail.com>

* Uncrustify linter

Signed-off-by: Wiktor Bajor <wiktorbajor1@gmail.com>

* Uncrustify linter

Signed-off-by: Wiktor Bajor <wiktorbajor1@gmail.com>

* Uncrustify linter: remove remove whitespace

Signed-off-by: Wiktor Bajor <wiktorbajor1@gmail.com>

---------

Signed-off-by: Wiktor Bajor <wiktorbajor1@gmail.com>

* Implement Critic for Velocity Deadband Hardware Constraints (#4256)

* Adding new velocity deadband critic.

- add some tests
- cast double to float
- add new features from "main" branch

- fix formating

- add cost test
- fix linting issue
- add README

Signed-off-by: Denis Sokolov <denis.sokolov48@gmail.com>

* Remove velocity deadband critic from defaults

Signed-off-by: Denis Sokolov <denis.sokolov48@gmail.com>

* remove old weight

Signed-off-by: Denis Sokolov <denis.sokolov48@gmail.com>

* fix velocity deadband critic tests

Signed-off-by: Denis Sokolov <denis.sokolov48@gmail.com>

---------

Signed-off-by: Denis Sokolov <denis.sokolov48@gmail.com>

* removing clearable layer param (unused) (#4280)

* provide message validation check API (#4276)

* provide validation_message.hpp

Signed-off-by: goes <GoesM@buaa.edu.cn>

* fix typo

Signed-off-by: goes <GoesM@buaa.edu.cn>

* add test_validation_messages.cpp

Signed-off-by: goes <GoesM@buaa.edu.cn>

* change include-order

Signed-off-by: goes <GoesM@buaa.edu.cn>

* reformat

Signed-off-by: goes <GoesM@buaa.edu.cn>

* update test

Signed-off-by: goes <GoesM@buaa.edu.cn>

---------

Signed-off-by: goes <GoesM@buaa.edu.cn>
Co-authored-by: goes <GoesM@buaa.edu.cn>

* [RotationShimController] rotate also on short paths (#4290)

When the path is shorter than ´forward_sampling_distance´ the
rotatitonShimController would directly give control to the primary
controller to navigate to the goal. This would lead to the exact
behavior that was tried to be fixed by the rotationShimController: "The
result is an awkward, stuttering, or whipping around behavior" [1]. It
often resulted in navigation failure.

In this case, the controller should try to rotate towards the last point
of the path, so that the primary controller can have a better starting
orientation.

[1] https://navigation.ros.org/tuning/index.html#rotate-in-place-behavior

* Add footprint clearing for static layer (#4282)

* Add footprint clearing for static layer

Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com>

* fix flckering

---------

Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com>

* Fix #4268 (#4296)

Signed-off-by: Steve Macenski <stevenmacenski@gmail.com>

* Fix undefined symbols in `libpf_lib.so` (#4312)

When I build `nav2_amcl` with `-Wl,--no-undefined` I noticed
`libpf_lib.so` has undefined symbols. This PR correctly links
`libpf_lib.so` to `libm` so all symbols can be found.

You can verify this by executing the following command:
```
ldd -r ./build/nav2_amcl/src/pf/libpf_lib.so
	linux-vdso.so.1 (0x00007ffd1f8c0000)
	libc.so.6 => /lib/x86_64-linux-gnu/libc.so.6 (0x000074e909a00000)
	/lib64/ld-linux-x86-64.so.2 (0x000074e909e60000)
undefined symbol: ceil	(./build/nav2_amcl/src/pf/libpf_lib.so)
undefined symbol: atan2	(./build/nav2_amcl/src/pf/libpf_lib.so)
undefined symbol: sin	(./build/nav2_amcl/src/pf/libpf_lib.so)
undefined symbol: hypot	(./build/nav2_amcl/src/pf/libpf_lib.so)
undefined symbol: cos	(./build/nav2_amcl/src/pf/libpf_lib.so)
undefined symbol: log	(./build/nav2_amcl/src/pf/libpf_lib.so)
undefined symbol: sqrt	(./build/nav2_amcl/src/pf/libpf_lib.so)
undefined symbol: floor	(./build/nav2_amcl/src/pf/libpf_lib.so)
```

Signed-off-by: Ramon Wijnands <ramon.wijnands@nobleo.nl>

* msg validation check for `/initialpose`  in `nav2_amcl` (#4301)

* add validation check for PoseWithCovarianceStamped

Signed-off-by: goes <GoesM@buaa.edu.cn>

* remove rebundant check before

Signed-off-by: goes <GoesM@buaa.edu.cn>

* reformat

Signed-off-by: goes <GoesM@buaa.edu.cn>

* typo fixed

Signed-off-by: goes <GoesM@buaa.edu.cn>

* change the type-name

Signed-off-by: goes <GoesM@buaa.edu.cn>

* update test

Signed-off-by: goes <GoesM@buaa.edu.cn>

* reformat

Signed-off-by: goes <GoesM@buaa.edu.cn>

* .

Signed-off-by: goes <GoesM@buaa.edu.cn>

* add comment

Signed-off-by: goes <GoesM@buaa.edu.cn>

* update comment

Signed-off-by: goes <GoesM@buaa.edu.cn>

* change header

Signed-off-by: goes <GoesM@buaa.edu.cn>

* update test

Signed-off-by: goes <GoesM@buaa.edu.cn>

* typo fixed

Signed-off-by: goes <GoesM@buaa.edu.cn>

---------

Signed-off-by: goes <GoesM@buaa.edu.cn>
Co-authored-by: goes <GoesM@buaa.edu.cn>

* 4320: Changed precision of calculations of the HybridNode MotionTable::getClosestAngularBin. (#4324)

Signed-off-by: Krzysztof Pawełczyk <k.t.pawelczyk@gmail.com>
Co-authored-by: Krzysztof Pawełczyk <kpawelczyk@autonomous-systems.pl>

* [LifecycleNode] add bond_heartbeat_period (#4342)

* add bond_heartbeat_period


Signed-off-by: Guillaume Doisy <guillaume@dexory.com>

* lint

Signed-off-by: Guillaume Doisy <guillaume@dexory.com>

---------

Signed-off-by: Guillaume Doisy <guillaume@dexory.com>
Co-authored-by: Guillaume Doisy <guillaume@dexory.com>

* Update path_longer_on_approach.cpp (#4344)

Fix the bug that isPathUpdated function will return false for the reason that it compare the timestaped between new path and old path's last pose

Signed-off-by: StetroF <120172218+StetroF@users.noreply.github.com>

* [LifecycleManagerClient] clean set_initial_pose and navigate_to_pose (#4346)

Signed-off-by: Guillaume Doisy <guillaume@dexory.com>
Co-authored-by: Guillaume Doisy <guillaume@dexory.com>

* Move projectState after getPointsInside (#4356)

* Modify test to check fix

Signed-off-by: Brice <brice.renaudeau@gmail.com>

* Add static polygon check before simulation

Signed-off-by: Brice <brice.renaudeau@gmail.com>

---------

Signed-off-by: Brice <brice.renaudeau@gmail.com>

* adding final pose in analytic expansion to check (#4353)

* bump iron to 1.2.8

* fix merge conflict resolution

---------

Signed-off-by: pepisg <pedro.gonzalez@eia.edu.co>
Signed-off-by: Wiktor Bajor <wiktorbajor1@gmail.com>
Signed-off-by: Denis Sokolov <denis.sokolov48@gmail.com>
Signed-off-by: goes <GoesM@buaa.edu.cn>
Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com>
Signed-off-by: Steve Macenski <stevenmacenski@gmail.com>
Signed-off-by: Ramon Wijnands <ramon.wijnands@nobleo.nl>
Signed-off-by: Krzysztof Pawełczyk <k.t.pawelczyk@gmail.com>
Signed-off-by: Guillaume Doisy <guillaume@dexory.com>
Signed-off-by: StetroF <120172218+StetroF@users.noreply.github.com>
Signed-off-by: Brice <brice.renaudeau@gmail.com>
Co-authored-by: Pedro Alejandro González <71234974+pepisg@users.noreply.github.com>
Co-authored-by: Wiktor Bajor <69388767+Wiktor-99@users.noreply.github.com>
Co-authored-by: Sokolov Denis <52282102+perchess@users.noreply.github.com>
Co-authored-by: GoesM <130988564+GoesM@users.noreply.github.com>
Co-authored-by: goes <GoesM@buaa.edu.cn>
Co-authored-by: Saitama <gennartan@users.noreply.github.com>
Co-authored-by: Tony Najjar <tony.najjar.1997@gmail.com>
Co-authored-by: Ramon Wijnands <ramon.wijnands@nobleo.nl>
Co-authored-by: AzaelCicero <k.t.pawelczyk@gmail.com>
Co-authored-by: Krzysztof Pawełczyk <kpawelczyk@autonomous-systems.pl>
Co-authored-by: Guillaume Doisy <doisyg@users.noreply.github.com>
Co-authored-by: Guillaume Doisy <guillaume@dexory.com>
Co-authored-by: StetroF <120172218+StetroF@users.noreply.github.com>
Co-authored-by: BriceRenaudeau <48433002+BriceRenaudeau@users.noreply.github.com>
SteveMacenski added a commit that referenced this issue May 23, 2024
* Scale cost critic's weight when dynamically updated (#4246)

* Scale cost critic's weight when dynamically updated

Signed-off-by: pepisg <pedro.gonzalez@eia.edu.co>

* sign off

Signed-off-by: pepisg <pedro.gonzalez@eia.edu.co>

---------

Signed-off-by: pepisg <pedro.gonzalez@eia.edu.co>

* Add expanding the ~/ to the full home dir of user in the path to the map yaml.  (#4258)

* Add user home expander of home sequence

Signed-off-by: Wiktor Bajor <wiktorbajor1@gmail.com>

* Add passing home dir as string instead of const char*

Signed-off-by: Wiktor Bajor <wiktorbajor1@gmail.com>

* Add docs

Signed-off-by: Wiktor Bajor <wiktorbajor1@gmail.com>

* Fix function declaration

Signed-off-by: Wiktor Bajor <wiktorbajor1@gmail.com>

* Fix linter issues

Signed-off-by: Wiktor Bajor <wiktorbajor1@gmail.com>

* Uncrustify linter

Signed-off-by: Wiktor Bajor <wiktorbajor1@gmail.com>

* Uncrustify linter

Signed-off-by: Wiktor Bajor <wiktorbajor1@gmail.com>

* Uncrustify linter: remove remove whitespace

Signed-off-by: Wiktor Bajor <wiktorbajor1@gmail.com>

---------

Signed-off-by: Wiktor Bajor <wiktorbajor1@gmail.com>

* Implement Critic for Velocity Deadband Hardware Constraints (#4256)

* Adding new velocity deadband critic.

- add some tests
- cast double to float
- add new features from "main" branch

- fix formating

- add cost test
- fix linting issue
- add README

Signed-off-by: Denis Sokolov <denis.sokolov48@gmail.com>

* Remove velocity deadband critic from defaults

Signed-off-by: Denis Sokolov <denis.sokolov48@gmail.com>

* remove old weight

Signed-off-by: Denis Sokolov <denis.sokolov48@gmail.com>

* fix velocity deadband critic tests

Signed-off-by: Denis Sokolov <denis.sokolov48@gmail.com>

---------

Signed-off-by: Denis Sokolov <denis.sokolov48@gmail.com>

* removing clearable layer param (unused) (#4280)

* provide message validation check API (#4276)

* provide validation_message.hpp

Signed-off-by: goes <GoesM@buaa.edu.cn>

* fix typo

Signed-off-by: goes <GoesM@buaa.edu.cn>

* add test_validation_messages.cpp

Signed-off-by: goes <GoesM@buaa.edu.cn>

* change include-order

Signed-off-by: goes <GoesM@buaa.edu.cn>

* reformat

Signed-off-by: goes <GoesM@buaa.edu.cn>

* update test

Signed-off-by: goes <GoesM@buaa.edu.cn>

---------

Signed-off-by: goes <GoesM@buaa.edu.cn>
Co-authored-by: goes <GoesM@buaa.edu.cn>

* Add footprint clearing for static layer (#4282)

* Add footprint clearing for static layer

Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com>

* fix flckering

---------

Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com>

* Fix #4268 (#4296)

Signed-off-by: Steve Macenski <stevenmacenski@gmail.com>

* Update README.md of nav2_bt_navigator (#4309)

Update link to docs

Signed-off-by: João Britto <cbn.joao@gmail.com>

* Fix undefined symbols in `libpf_lib.so` (#4312)

When I build `nav2_amcl` with `-Wl,--no-undefined` I noticed
`libpf_lib.so` has undefined symbols. This PR correctly links
`libpf_lib.so` to `libm` so all symbols can be found.

You can verify this by executing the following command:
```
ldd -r ./build/nav2_amcl/src/pf/libpf_lib.so
	linux-vdso.so.1 (0x00007ffd1f8c0000)
	libc.so.6 => /lib/x86_64-linux-gnu/libc.so.6 (0x000074e909a00000)
	/lib64/ld-linux-x86-64.so.2 (0x000074e909e60000)
undefined symbol: ceil	(./build/nav2_amcl/src/pf/libpf_lib.so)
undefined symbol: atan2	(./build/nav2_amcl/src/pf/libpf_lib.so)
undefined symbol: sin	(./build/nav2_amcl/src/pf/libpf_lib.so)
undefined symbol: hypot	(./build/nav2_amcl/src/pf/libpf_lib.so)
undefined symbol: cos	(./build/nav2_amcl/src/pf/libpf_lib.so)
undefined symbol: log	(./build/nav2_amcl/src/pf/libpf_lib.so)
undefined symbol: sqrt	(./build/nav2_amcl/src/pf/libpf_lib.so)
undefined symbol: floor	(./build/nav2_amcl/src/pf/libpf_lib.so)
```

Signed-off-by: Ramon Wijnands <ramon.wijnands@nobleo.nl>

* msg validation check for `/initialpose`  in `nav2_amcl` (#4301)

* add validation check for PoseWithCovarianceStamped

Signed-off-by: goes <GoesM@buaa.edu.cn>

* remove rebundant check before

Signed-off-by: goes <GoesM@buaa.edu.cn>

* reformat

Signed-off-by: goes <GoesM@buaa.edu.cn>

* typo fixed

Signed-off-by: goes <GoesM@buaa.edu.cn>

* change the type-name

Signed-off-by: goes <GoesM@buaa.edu.cn>

* update test

Signed-off-by: goes <GoesM@buaa.edu.cn>

* reformat

Signed-off-by: goes <GoesM@buaa.edu.cn>

* .

Signed-off-by: goes <GoesM@buaa.edu.cn>

* add comment

Signed-off-by: goes <GoesM@buaa.edu.cn>

* update comment

Signed-off-by: goes <GoesM@buaa.edu.cn>

* change header

Signed-off-by: goes <GoesM@buaa.edu.cn>

* update test

Signed-off-by: goes <GoesM@buaa.edu.cn>

* typo fixed

Signed-off-by: goes <GoesM@buaa.edu.cn>

---------

Signed-off-by: goes <GoesM@buaa.edu.cn>
Co-authored-by: goes <GoesM@buaa.edu.cn>

* 4320: Changed precision of calculations of the HybridNode MotionTable::getClosestAngularBin. (#4324)

Signed-off-by: Krzysztof Pawełczyk <k.t.pawelczyk@gmail.com>
Co-authored-by: Krzysztof Pawełczyk <kpawelczyk@autonomous-systems.pl>

* [LifecycleNode] add bond_heartbeat_period (#4342)

* add bond_heartbeat_period


Signed-off-by: Guillaume Doisy <guillaume@dexory.com>

* lint

Signed-off-by: Guillaume Doisy <guillaume@dexory.com>

---------

Signed-off-by: Guillaume Doisy <guillaume@dexory.com>
Co-authored-by: Guillaume Doisy <guillaume@dexory.com>

* Update path_longer_on_approach.cpp (#4344)

Fix the bug that isPathUpdated function will return false for the reason that it compare the timestaped between new path and old path's last pose

Signed-off-by: StetroF <120172218+StetroF@users.noreply.github.com>

* [LifecycleManagerClient] clean set_initial_pose and navigate_to_pose (#4346)

Signed-off-by: Guillaume Doisy <guillaume@dexory.com>
Co-authored-by: Guillaume Doisy <guillaume@dexory.com>

* Move projectState after getPointsInside (#4356)

* Modify test to check fix

Signed-off-by: Brice <brice.renaudeau@gmail.com>

* Add static polygon check before simulation

Signed-off-by: Brice <brice.renaudeau@gmail.com>

---------

Signed-off-by: Brice <brice.renaudeau@gmail.com>

* adding final pose in analytic expansion to check (#4353)

* fix sync merge conflicts

* bump humble to 1.1.15 for release

* Revert "[LifecycleNode] add bond_heartbeat_period (#4342)"

This reverts commit 6e44178.

---------

Signed-off-by: pepisg <pedro.gonzalez@eia.edu.co>
Signed-off-by: Wiktor Bajor <wiktorbajor1@gmail.com>
Signed-off-by: Denis Sokolov <denis.sokolov48@gmail.com>
Signed-off-by: goes <GoesM@buaa.edu.cn>
Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com>
Signed-off-by: Steve Macenski <stevenmacenski@gmail.com>
Signed-off-by: João Britto <cbn.joao@gmail.com>
Signed-off-by: Ramon Wijnands <ramon.wijnands@nobleo.nl>
Signed-off-by: Krzysztof Pawełczyk <k.t.pawelczyk@gmail.com>
Signed-off-by: Guillaume Doisy <guillaume@dexory.com>
Signed-off-by: StetroF <120172218+StetroF@users.noreply.github.com>
Signed-off-by: Brice <brice.renaudeau@gmail.com>
Co-authored-by: Pedro Alejandro González <71234974+pepisg@users.noreply.github.com>
Co-authored-by: Wiktor Bajor <69388767+Wiktor-99@users.noreply.github.com>
Co-authored-by: Sokolov Denis <52282102+perchess@users.noreply.github.com>
Co-authored-by: GoesM <130988564+GoesM@users.noreply.github.com>
Co-authored-by: goes <GoesM@buaa.edu.cn>
Co-authored-by: Tony Najjar <tony.najjar.1997@gmail.com>
Co-authored-by: João Britto <cbn.joao@gmail.com>
Co-authored-by: Ramon Wijnands <ramon.wijnands@nobleo.nl>
Co-authored-by: AzaelCicero <k.t.pawelczyk@gmail.com>
Co-authored-by: Krzysztof Pawełczyk <kpawelczyk@autonomous-systems.pl>
Co-authored-by: Guillaume Doisy <doisyg@users.noreply.github.com>
Co-authored-by: Guillaume Doisy <guillaume@dexory.com>
Co-authored-by: StetroF <120172218+StetroF@users.noreply.github.com>
Co-authored-by: BriceRenaudeau <48433002+BriceRenaudeau@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

No branches or pull requests

3 participants