Set state fix #785
Open
Set state fix #785
Conversation
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
ayrton04
requested changes
Jun 5, 2023
| geometry_msgs/PoseWithCovarianceStamped pose | ||
| geometry_msgs/TwistWithCovarianceStamped twist | ||
| geometry_msgs/AccelWithCovarianceStamped accel | ||
| bool[15] update_vector |
| @@ -51,7 +51,7 @@ | |||
| #include <utility> | |||
| #include <memory> | |||
| #include <vector> | |||
|
|
|||
| #include <Eigen/Dense> | |||
There was a problem hiding this comment.
Removed line break here. Please replace.
| @@ -1035,13 +1036,26 @@ void RosFilter<T>::loadParams() | |||
| "set_pose", rclcpp::QoS(1), | |||
| std::bind(&RosFilter<T>::setPoseCallback, this, std::placeholders::_1)); | |||
|
|
|||
| // Create a subscriber for manually setting/resetting state | |||
| set_state_sub_ = | |||
There was a problem hiding this comment.
Do we want to support this via messages? This feels like a service call should suffice.
| @@ -0,0 +1,3 @@ | |||
| State state | |||
| --- | |||
| bool success | |||
| @@ -2243,6 +2257,148 @@ bool RosFilter<T>::setPoseSrvCallback( | |||
| return true; | |||
| } | |||
|
|
|||
| template<typename T> | |||
| void RosFilter<T>::setStateCallback(const robot_localization::msg::State::SharedPtr msg) | |||
There was a problem hiding this comment.
I feel like this duplicates a lot of code. Why can't we just call reset() and then set the state? As much as I appreciate the input sanitising, I think the user needs to be ensuring that they are sending valid states to the filter. This class is already too bloated, and this method adds a lot of new stuff.
| all_real_and_nonnegative = all_real_and_nonnegative | ||
| && eigval.real()>0.0 && std::abs(eigval.imag())<1E-6; | ||
| } | ||
| if(all_real_and_nonnegative){ |
| all_real_and_nonnegative = all_real_and_nonnegative | ||
| && eigval.real()>0.0 && std::abs(eigval.imag())<1E-6; | ||
| } | ||
| if(all_real_and_nonnegative){ |
| }else{ | ||
| RCLCPP_INFO(get_logger(), "Not setting acceleration because acceleration covariance is not positive definite"); | ||
| } | ||
| }else{ |
There was a problem hiding this comment.
All these else cases should have spaces between the braces and the else.
| RCLCPP_INFO(get_logger(), "Setting acceleration"); | ||
| std::shared_ptr<sensor_msgs::msg::Imu> imu_ptr(new sensor_msgs::msg::Imu); | ||
| std::vector<bool> update_vector_imu(msg->update_vector.begin(), msg->update_vector.end()); | ||
| update_vector_imu[StateMemberVroll]=update_vector_imu[StateMemberVpitch]=update_vector_imu[StateMemberVyaw]= |
There was a problem hiding this comment.
This is an ugly line. Can we clean this up?
Sign up for free
to join this conversation on GitHub.
Already have an account?
Sign in to comment
Add this suggestion to a batch that can be applied as a single commit.
This suggestion is invalid because no changes were made to the code.
Suggestions cannot be applied while the pull request is closed.
Suggestions cannot be applied while viewing a subset of changes.
Only one suggestion per line can be applied in a batch.
Add this suggestion to a batch that can be applied as a single commit.
Applying suggestions on deleted lines is not supported.
You must change the existing code in this line in order to create a valid suggestion.
Outdated suggestions cannot be applied.
This suggestion has been applied or marked resolved.
Suggestions cannot be applied from pending reviews.
Suggestions cannot be applied on multi-line comments.
Suggestions cannot be applied while the pull request is queued to merge.
Suggestion cannot be applied right now. Please check back later.
No description provided.