Class state::StateEstimator
-
class StateEstimator
State Estimator! Provides functions for controller node to use to prepare state information for mppi. Implemnentation is in StateEstimator_Impl. Refer to the.
explainer for a more detailed overview.
Subclassed by controls::state::StateEstimator_Impl
Public Functions
-
virtual void sync_to_device(const rclcpp::Time &time) = 0
“main” function of the state estimator. Calculates current inertial state and the inertial-to-curvilinear lookup table, then syncs them to the GPU for MPPI use. (into cuda_globals::curv_frame_lookup_tex and cuda_globals::curr_state respectively)
- Parameters:
time – The current time
-
virtual void on_spline(const SplineMsg &spline_msg) = 0
Callback for spline messages. Updates the state estimator with the new spline. Used for both state projection and curvilinear state lookup table generation.
Note
Time does not need to be passed in since that is encoded in the spline message (
spline_orig_data_stamp), and is inaccessible to the caller anyway.- Parameters:
spline_msg – The spline message
-
virtual void on_twist(const TwistMsg &twist_msg, const rclcpp::Time &time) = 0
Callback for twist messages. Updates the state estimator with the new twist. Used for state projection.
- Parameters:
twist_msg – The spline message
time – The time the twist is accurate to.
-
virtual void on_pose(const PoseMsg &pose_msg) = 0
Callback for pose messages. Can be used for state projection, but currently unused. Reason: position given by spline instead, yaw given by steering wheel angle. Pose meanwhile is noisy?
- Parameters:
pose_msg – The pose message
-
virtual bool is_ready() = 0
Returns whether state projection is ready. If not, the MPPI controller should wait. This should only be not ready when the state estimator is first initialized and the controller has no spline.
- Returns:
True if state projection is ready, False otherwise.
-
virtual State get_projected_state() = 0
Returns the last state that was projected. Does not actually do the projection (that is in an internal call in sync_to_device). Used for debugging pursposes.
Note
Spline provides state information since car is at (0, 0) relative to a new spline.
- Returns:
The most recent projected state.
-
virtual void set_logger(LoggerFunc logger) = 0
Sets the logger function for the state estimator. Can be integrated with ROS or print to stdout.
- Parameters:
logger – The logger function to be used.
-
virtual rclcpp::Time get_orig_spline_data_stamp() = 0
Returns the timestamp of the last spline message received. This is accurate to when the sensor (i.e. LIDAR) information was received, not when the spline was calculated.
- Returns:
The timestamp of the last spline message received.
-
virtual void record_control_action(const Action &action, const rclcpp::Time &ros_time) = 0
Records a control action for state projection purposes.
- Parameters:
action – The control action to record
ros_time – The time the action was recorded
-
virtual ~StateEstimator() = default
Virtual destructor for the state estimator.
Public Static Functions
-
static std::shared_ptr<StateEstimator> create(std::mutex &mutex, LoggerFunc logger = no_log)
Essentially serves as a named constructor that grants ownership of a StateEstimator to the caller. This is necessary because StateEstimator is an abstract base class (a pointer is needed)
- Parameters:
mutex – [in] Reference to the mutex that state estimator will use. Prevents any public function from being called concurrently, which would cause undefined behavior of enabling the same CUDA context.
logger – [in] Function object of logger (string to void)
- Returns:
Pointer to the created StateEstimator
-
virtual void sync_to_device(const rclcpp::Time &time) = 0