reference filter mode dependant pose convergence#664
Conversation
Codecov Report❌ Patch coverage is Additional details and impacted files@@ Coverage Diff @@
## main #664 +/- ##
==========================================
+ Coverage 34.64% 35.44% +0.80%
==========================================
Files 43 43
Lines 2823 2835 +12
Branches 749 757 +8
==========================================
+ Hits 978 1005 +27
+ Misses 1663 1647 -16
- Partials 182 183 +1
Flags with carried forward coverage won't be shown. Click here to find out more.
🚀 New features to boost your workflow:
|
| bool ReferenceFilterNode::has_converged_against_pose( | ||
| const Eigen::Vector6d& y, | ||
| const Eigen::Vector6d& r, | ||
| uint8_t mode, | ||
| double convergence_threshold) const { | ||
| const Eigen::Vector3d ep = y.head<3>() - r.head<3>(); | ||
|
|
||
| const auto& p = current_pose_.pose.pose.position; | ||
| const auto& o = current_pose_.pose.pose.orientation; | ||
| Eigen::Vector3d ea; | ||
| ea(0) = vortex::utils::math::ssa(y(3) - r(3)); | ||
| ea(1) = vortex::utils::math::ssa(y(4) - r(4)); | ||
| ea(2) = vortex::utils::math::ssa(y(5) - r(5)); |
There was a problem hiding this comment.
The variable naming was already bad in this package, but you arent really contributing here 😆
| Eigen::Vector6d y; | ||
| y << p.x, p.y, p.z, vortex::utils::math::ssa(euler(0)), | ||
| vortex::utils::math::ssa(euler(1)), vortex::utils::math::ssa(euler(2)); |
There was a problem hiding this comment.
Could also construct it in one go
using vortex::utils::math::ssa;
Eigen::Vector6d y{p.x, p.y, p.z, ssa(euler(0)), ssa(euler(1)), ssa(euler(2)));
| bool ReferenceFilterNode::has_converged_against_pose( | ||
| const Eigen::Vector6d& y, | ||
| const Eigen::Vector6d& r, | ||
| uint8_t mode, | ||
| double convergence_threshold) const { |
There was a problem hiding this comment.
Could you be explicit and take mode as Enum? Requires a conversion between ros msg and enum, but thats no problem
There was a problem hiding this comment.
Recently came across cppyy, which allows you to run c++ code in python. You may find that interesting
|
🎉 This PR is included in version 2.6.0 🎉 The release is available on GitHub release Your semantic-release bot 📦🚀 |
2 participants
Converted the convergence check to use current pose instead of x_.
Also mode the convergence logic mode dependant