-
Notifications
You must be signed in to change notification settings - Fork 9
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
WIP: Generic state estimator #206
base: master
Are you sure you want to change the base?
Conversation
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
Need a connector to own both front end and back end and expose a status function and state getter
Reviewed 5 of 5 files at r1.
Reviewable status: all files reviewed, 7 unresolved discussions (waiting on @garimellagowtham)
include/aerial_autonomy/estimators/generic_state_estimator_back_end.h, line 10 at r1 (raw file):
#include <vector> class GenericStateEstimatorBackend {
Just call it StateEstimatorBackend
include/aerial_autonomy/estimators/generic_state_estimator_back_end.h, line 26 at r1 (raw file):
const geometry_msgs::Vector3 acceleration); void addAltitude(time_point sensor_time, const double altitude); void addTargetPosition(time_point sensor_time,
Should just be one function called addTargetPose
include/aerial_autonomy/estimators/generic_state_estimator_back_end.h, line 31 at r1 (raw file):
const geometry_msgs::Quaternion target_orientation); // Optimize based on available data void optimize();
should have an optimization status
include/aerial_autonomy/estimators/generic_state_estimator_back_end.h, line 36 at r1 (raw file):
tf::StampedTransform getTargetPose(); tf::Stamped<tf::Vector3> getBodyVelocity(); tf::Stamped<tf::Vector3> getSpatialVelocity();
we can implement our own Stamped for std::chrono
include/aerial_autonomy/estimators/generic_state_estimator_back_end.h, line 41 at r1 (raw file):
protected: template <class T> using DequeVector = std::deque<std::vector<T>>; DequeVector<geometry_msgs::Vector3> position_deque;
we should use eigen instead geometry_msgs (easier to map to **)
include/aerial_autonomy/estimators/generic_state_estimator_front_end.h, line 17 at r1 (raw file):
GenericStateEstimatorBackend &back_end); void trackingVectorCallback(time_point sensor_time, tf::Transform pose); void poseCallback(const geometry_msgs::TransformStamped &pose_input);
some of these will need the proper ROS callback parametrization
include/aerial_autonomy/estimators/generic_state_estimator_front_end.h, line 22 at r1 (raw file):
void accCallback(time_point sensor_time, const geometry_msgs::Vector3 &acc); void altitudeCallback(time_point sensor_time, const double &altitude); void guidanceCallback(const geometry_msgs::Vector3Stamped &velocity);
need a status function
Create a state estimator that own frontend and backend
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
Reviewable status: 2 of 7 files reviewed, 7 unresolved discussions (waiting on @msheckells)
include/aerial_autonomy/estimators/generic_state_estimator_back_end.h, line 10 at r1 (raw file):
Previously, msheckells (Matt Sheckells) wrote…
Just call it StateEstimatorBackend
Done.
include/aerial_autonomy/estimators/generic_state_estimator_back_end.h, line 26 at r1 (raw file):
Previously, msheckells (Matt Sheckells) wrote…
Should just be one function called addTargetPose
Done.
include/aerial_autonomy/estimators/generic_state_estimator_back_end.h, line 31 at r1 (raw file):
Previously, msheckells (Matt Sheckells) wrote…
should have an optimization status
Done.
include/aerial_autonomy/estimators/generic_state_estimator_back_end.h, line 36 at r1 (raw file):
Previously, msheckells (Matt Sheckells) wrote…
we can implement our own Stamped for std::chrono
Done.
include/aerial_autonomy/estimators/generic_state_estimator_back_end.h, line 41 at r1 (raw file):
Previously, msheckells (Matt Sheckells) wrote…
we should use eigen instead geometry_msgs (easier to map to **)
Done.
include/aerial_autonomy/estimators/generic_state_estimator_front_end.h, line 17 at r1 (raw file):
Previously, msheckells (Matt Sheckells) wrote…
some of these will need the proper ROS callback parametrization
Done.
include/aerial_autonomy/estimators/generic_state_estimator_front_end.h, line 22 at r1 (raw file):
Previously, msheckells (Matt Sheckells) wrote…
need a status function
Done.
Add callback functionality to tracker
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
Reviewed 17 of 17 files at r2.
Reviewable status: all files reviewed, 6 unresolved discussions (waiting on @garimellagowtham)
include/aerial_autonomy/common/chrono_stamped.h, line 9 at r2 (raw file):
using time_point = std::chrono::time_point<std::chrono::high_resolution_clock>; time_point stamp;
should this be public? Do we ever expect to have to reset the stamp?
include/aerial_autonomy/common/chrono_stamped.h, line 15 at r2 (raw file):
Stamped(const T &input) : Stamped(input, std::chrono::high_resolution_clock::now()) {} void setData(const T &input) { *static_cast<T *>(this) = input; };
This is interesting. So this resets the T part of the class without resetting stamp?
include/aerial_autonomy/estimators/state_estimator_backend.h, line 16 at r2 (raw file):
using time_point = std::chrono::time_point<std::chrono::high_resolution_clock>; void addPositionData(time_point sensor_time, const Eigen::Vector3d position);
call it addPosition to be consistent
include/aerial_autonomy/estimators/state_estimator_backend.h, line 31 at r2 (raw file):
protected: template <class T> using DequeVector = std::deque<std::vector<T>>; DequeVector<Eigen::Vector3d> position_deque;
The DequeVector should contain a Stamped so that we keep the time stamps
include/aerial_autonomy/estimators/state_estimator_status.h, line 4 at r2 (raw file):
struct CumulativeSensorStatus { enum class SensorStatus { Active = 0, Inactive = 1, Critical = 2 };
is SensorStatus defined somewhere else?
include/aerial_autonomy/estimators/state_estimator_status.h, line 15 at r2 (raw file):
CumulativeSensorStatus sensor_status; OptimizationStatus optimization_status = OptimizationStatus::SUCCESS; bool estimator_status = false;
should this be a SensorStatus?
This change is