gz-rviz icon indicating copy to clipboard operation
gz-rviz copied to clipboard

Published 20 hours ago • verified publisher icon gazebosim

Pose with covariance display

Open Thodoris1999 opened this issue 2 years ago • 7 comments

🎉 New feature

PoseWithCovariance display, largely inspired by the original implementation in RViz

More info in this post https://community.gazebosim.org/t/gsoc-2022-potential-project-ideas/1339/4

Checklist

  • [ ] Signed all commits for DCO
  • [ ] Added tests
  • [ ] Added example and/or tutorial
  • [ ] Updated documentation (as needed)
  • [ ] Updated migration guide (as needed)
  • [ ] codecheck passed (See contributing)
  • [ ] All tests passed (See test coverage)
  • [ ] While waiting for a review on your PR, please help review another open pull request to support the maintainers

Note to maintainers: Remember to use Squash-Merge and edit the commit message to match the pull request summary while retaining Signed-off-by messages.

Thodoris1999 avatar Apr 18 '22 17:04 Thodoris1999

This pull request has been mentioned on Gazebo Community. There might be relevant details there:

https://community.gazebosim.org/t/gsoc-2022-potential-project-ideas/1339/4

osrf-triage avatar Apr 18 '22 17:04 osrf-triage

Can you help me with how to remove those changes from history? I haven't done this before and don't want to accidentally mess anything up.

Thodoris1999 avatar Apr 20 '22 16:04 Thodoris1999

@ahcorde I dropped the "Add fortress support" commit with rebase

Thodoris1999 avatar Apr 21 '22 14:04 Thodoris1999

Thank you for the review! I will get to the changes soon.

As for the math, I used Eigen to be on a safe side with performance. I use diagonalization, which is very optimized in Eigen with a LAPACK implementation that takes advantage of the symmetric covariance https://eigen.tuxfamily.org/dox-devel/classEigen_1_1SelfAdjointEigenSolver.html#a110f7f5ff19fcabe6a10c6d48d5c419c. MassMatrix uses a diagonalization implementation https://github.com/ignitionrobotics/ign-math/blob/c08c659229777e32d684841aea8fdcc1bdc57c4f/include/ignition/math/MassMatrix3.hh#L646 for calculating the principal axes with ign-math but uses two separate functions for eigenvalues and eigenvectors. Maybe those two implementation could be merged into one and extracted to an outside function that takes a Matrix3 and returns the right-handed diagonalization.

Thodoris1999 avatar Apr 22 '22 06:04 Thodoris1999

@ahcorde Sorry to bother, but I am having some issues using PIMPL with utils::MakeImpl<Implementation>() in the last commit I pushed, apparently somewhere there is an unwanted copy construction. Do these compiler errors maybe ring a bell to you?

--- stderr: ign_rviz_plugins
In file included from /home/thodoris/src/ignition/install/include/ignition/utils1/ignition/utils/ImplPtr.hh:24,
                 from /home/thodoris/src/ignition/install/include/ignition/common4/ignition/common/Pbr.hh:22,
                 from /home/thodoris/src/ignition/install/include/ignition/common4/ignition/common/Material.hh:27,
                 from /home/thodoris/src/ignition/install/include/ignition/rendering5/ignition/rendering/Scene.hh:24,
                 from /home/thodoris/src/ignition/install/include/ignition/rendering5/ignition/rendering/base/BaseArrowVisual.hh:21,
                 from /home/thodoris/src/ignition/install/include/ignition/rendering5/ignition/rendering.hh:23,
                 from /home/thodoris/src/ignition/src/ign-rviz/ign_rviz_plugins/include/ignition/rviz/plugins/PoseWithCovarianceDisplay.hpp:18,
                 from /home/thodoris/src/ignition/src/ign-rviz/ign_rviz_plugins/src/rviz/plugins/PoseWithCovarianceDisplay.cpp:15:
/home/thodoris/src/ignition/install/include/ignition/utils1/ignition/utils/detail/DefaultOps.hh: In instantiation of ‘T* ignition::utils::detail::DefaultCopyConstruct(const T&) [with T = ignition::rviz::plugins::PoseWithCovarianceDisplay::Implementation]’:
/home/thodoris/src/ignition/install/include/ignition/utils1/ignition/utils/detail/ImplPtr.hh:135:21:   required from ‘ignition::utils::ImplPtr<T> ignition::utils::MakeImpl(Args&& ...) [with T = ignition::rviz::plugins::PoseWithCovarianceDisplay::Implementation; Args = {}]’
/home/thodoris/src/ignition/src/ign-rviz/ign_rviz_plugins/src/rviz/plugins/PoseWithCovarianceDisplay.cpp:68:61:   required from here
/home/thodoris/src/ignition/install/include/ignition/utils1/ignition/utils/detail/DefaultOps.hh:56:16: error: use of deleted function ‘ignition::rviz::plugins::PoseWithCovarianceDisplay::Implementation::Implementation(const ignition::rviz::plugins::PoseWithCovarianceDisplay::Implementation&)’
   56 |         return new T(_source);
      |                ^~~~~~~~~~~~~~
/home/thodoris/src/ignition/src/ign-rviz/ign_rviz_plugins/src/rviz/plugins/PoseWithCovarianceDisplay.cpp:50:34: note: ‘ignition::rviz::plugins::PoseWithCovarianceDisplay::Implementation::Implementation(const ignition::rviz::plugins::PoseWithCovarianceDisplay::Implementation&)’ is implicitly deleted because the default definition would be ill-formed:
   50 | class PoseWithCovarianceDisplay::Implementation
      |                                  ^~~~~~~~~~~~~~
/home/thodoris/src/ignition/src/ign-rviz/ign_rviz_plugins/src/rviz/plugins/PoseWithCovarianceDisplay.cpp:50:34: error: use of deleted function ‘std::mutex::mutex(const std::mutex&)’
In file included from /usr/include/c++/9/condition_variable:39,
                 from /usr/include/c++/9/shared_mutex:37,
                 from /usr/include/c++/9/memory_resource:41,
                 from /usr/include/c++/9/regex:66,
                 from /home/thodoris/src/ignition/install/include/ignition/math6/ignition/math/Helpers.hh:27,
                 from /home/thodoris/src/ignition/install/include/ignition/math6/ignition/math/AxisAlignedBox.hh:23,
                 from /home/thodoris/src/ignition/install/include/ignition/rendering5/ignition/rendering/Visual.hh:22,
                 from /home/thodoris/src/ignition/install/include/ignition/rendering5/ignition/rendering/CompositeVisual.hh:21,
                 from /home/thodoris/src/ignition/install/include/ignition/rendering5/ignition/rendering/ArrowVisual.hh:21,
                 from /home/thodoris/src/ignition/install/include/ignition/rendering5/ignition/rendering/base/BaseArrowVisual.hh:20,
                 from /home/thodoris/src/ignition/install/include/ignition/rendering5/ignition/rendering.hh:23,
                 from /home/thodoris/src/ignition/src/ign-rviz/ign_rviz_plugins/include/ignition/rviz/plugins/PoseWithCovarianceDisplay.hpp:18,
                 from /home/thodoris/src/ignition/src/ign-rviz/ign_rviz_plugins/src/rviz/plugins/PoseWithCovarianceDisplay.cpp:15:
/usr/include/c++/9/bits/std_mutex.h:94:5: note: declared here
   94 |     mutex(const mutex&) = delete;
      |     ^~~~~
In file included from /home/thodoris/src/ignition/install/include/ignition/utils1/ignition/utils/ImplPtr.hh:24,
                 from /home/thodoris/src/ignition/install/include/ignition/common4/ignition/common/Pbr.hh:22,
                 from /home/thodoris/src/ignition/install/include/ignition/common4/ignition/common/Material.hh:27,
                 from /home/thodoris/src/ignition/install/include/ignition/rendering5/ignition/rendering/Scene.hh:24,
                 from /home/thodoris/src/ignition/install/include/ignition/rendering5/ignition/rendering/base/BaseArrowVisual.hh:21,
                 from /home/thodoris/src/ignition/install/include/ignition/rendering5/ignition/rendering.hh:23,
                 from /home/thodoris/src/ignition/src/ign-rviz/ign_rviz_plugins/include/ignition/rviz/plugins/PoseWithCovarianceDisplay.hpp:18,
                 from /home/thodoris/src/ignition/src/ign-rviz/ign_rviz_plugins/src/rviz/plugins/PoseWithCovarianceDisplay.cpp:15:
/home/thodoris/src/ignition/install/include/ignition/utils1/ignition/utils/detail/DefaultOps.hh: In instantiation of ‘void ignition::utils::detail::DefaultCopyAssign(T&, const T&) [with T = ignition::rviz::plugins::PoseWithCovarianceDisplay::Implementation]’:
/home/thodoris/src/ignition/install/include/ignition/utils1/ignition/utils/detail/ImplPtr.hh:135:21:   required from ‘ignition::utils::ImplPtr<T> ignition::utils::MakeImpl(Args&& ...) [with T = ignition::rviz::plugins::PoseWithCovarianceDisplay::Implementation; Args = {}]’
/home/thodoris/src/ignition/src/ign-rviz/ign_rviz_plugins/src/rviz/plugins/PoseWithCovarianceDisplay.cpp:68:61:   required from here
/home/thodoris/src/ignition/install/include/ignition/utils1/ignition/utils/detail/DefaultOps.hh:64:15: error: use of deleted function ‘ignition::rviz::plugins::PoseWithCovarianceDisplay::Implementation& ignition::rviz::plugins::PoseWithCovarianceDisplay::Implementation::operator=(const ignition::rviz::plugins::PoseWithCovarianceDisplay::Implementation&)’
   64 |         _dest = _source;
      |         ~~~~~~^~~~~~~~~
/home/thodoris/src/ignition/src/ign-rviz/ign_rviz_plugins/src/rviz/plugins/PoseWithCovarianceDisplay.cpp:50:34: note: ‘ignition::rviz::plugins::PoseWithCovarianceDisplay::Implementation& ignition::rviz::plugins::PoseWithCovarianceDisplay::Implementation::operator=(const ignition::rviz::plugins::PoseWithCovarianceDisplay::Implementation&)’ is implicitly deleted because the default definition would be ill-formed:
   50 | class PoseWithCovarianceDisplay::Implementation
      |                                  ^~~~~~~~~~~~~~
/home/thodoris/src/ignition/src/ign-rviz/ign_rviz_plugins/src/rviz/plugins/PoseWithCovarianceDisplay.cpp:50:34: error: use of deleted function ‘std::mutex& std::mutex::operator=(const std::mutex&)’
In file included from /usr/include/c++/9/condition_variable:39,
                 from /usr/include/c++/9/shared_mutex:37,
                 from /usr/include/c++/9/memory_resource:41,
                 from /usr/include/c++/9/regex:66,
                 from /home/thodoris/src/ignition/install/include/ignition/math6/ignition/math/Helpers.hh:27,
                 from /home/thodoris/src/ignition/install/include/ignition/math6/ignition/math/AxisAlignedBox.hh:23,
                 from /home/thodoris/src/ignition/install/include/ignition/rendering5/ignition/rendering/Visual.hh:22,
                 from /home/thodoris/src/ignition/install/include/ignition/rendering5/ignition/rendering/CompositeVisual.hh:21,
                 from /home/thodoris/src/ignition/install/include/ignition/rendering5/ignition/rendering/ArrowVisual.hh:21,
                 from /home/thodoris/src/ignition/install/include/ignition/rendering5/ignition/rendering/base/BaseArrowVisual.hh:20,
                 from /home/thodoris/src/ignition/install/include/ignition/rendering5/ignition/rendering.hh:23,
                 from /home/thodoris/src/ignition/src/ign-rviz/ign_rviz_plugins/include/ignition/rviz/plugins/PoseWithCovarianceDisplay.hpp:18,
                 from /home/thodoris/src/ignition/src/ign-rviz/ign_rviz_plugins/src/rviz/plugins/PoseWithCovarianceDisplay.cpp:15:
/usr/include/c++/9/bits/std_mutex.h:95:12: note: declared here
   95 |     mutex& operator=(const mutex&) = delete;
      |            ^~~~~~~~
make[2]: *** [CMakeFiles/PoseWithCovarianceDisplay.dir/build.make:97: CMakeFiles/PoseWithCovarianceDisplay.dir/src/rviz/plugins/PoseWithCovarianceDisplay.cpp.o] Error 1
make[1]: *** [CMakeFiles/Makefile2:178: CMakeFiles/PoseWithCovarianceDisplay.dir/all] Error 2
make: *** [Makefile:141: all] Error 2
---
Failed   <<< ign_rviz_plugins [11.3s, exited with code 2]

Thodoris1999 avatar Apr 22 '22 14:04 Thodoris1999

@ahcorde Did the merge

By the way, the error I sent above was due to std::mutex not being copy-assignable. We can wrap it with std::shared_ptr and that fixes it, but I am not sure if it's the cleanest solution. Shouldn't the PoseWithCovariance::Implementation not be copy-assigned in the first place?

Thodoris1999 avatar Apr 25 '22 18:04 Thodoris1999

Hi, I came back after catching up to school work. I ended up using a shared pointer for the mutex.

I tried to address the linter warnings:

  • cpplint erroneously considers ROS distribution includes (messages, TF etc) as C system dependencies and complains that they should be included before C++ system includes. I guess these warnings were ignored since they exist for the entire source code.
  • cpplint and uncrustify seem to conflict in the following piece of code:
if (user_data_.position_frame == Frame::Local &&
    fixed_orientation_visual_->HasChild(position_root_visual_)) {
    this->root_visual_->AddChild(fixed_orientation_visual_->RemoveChild(position_root_visual_));
} else if (user_data_.position_frame == Frame::Fixed &&
    root_visual_->HasChild(position_root_visual_)) {
    fixed_orientation_visual_->AddChild(this->root_visual_->RemoveChild(position_root_visual_));
}
if (user_data_.orientation_frame == Frame::Local &&
    fixed_orientation_visual_->HasChild(orientation_root_visual_)) {
    this->root_visual_->AddChild(fixed_orientation_visual_->RemoveChild(orientation_root_visual_));
} else if (user_data_.orientation_frame == Frame::Fixed &&
    root_visual_->HasChild(orientation_root_visual_)) {
    fixed_orientation_visual_->AddChild(this->root_visual_->RemoveChild(orientation_root_visual_));
}

uncrustify wants the else if's opening brackets to be placed on the next line, while cpplint wants "else statements have one bracket on the same line they should have both". Is there a way to fix this?

  • The rest of the errors where fixed.

Finally, there seems to be a DCO error:

Summary

Commit sha: [b7f3ef7](https://github.com/gazebosim/gz-rviz/pull/76/commits/b7f3ef7a1d71d4d8883709946d5e8f1e80da9e6b), Author: Alejandro Hernández Cordero, Committer: Theodoros Tyrovouzis; Expected "Alejandro Hernández Cordero [[email protected]](mailto:[email protected])", but got "ahcorde [[email protected]](mailto:[email protected])".

Thodoris1999 avatar May 09 '22 16:05 Thodoris1999