Class state::StateEstimator_Impl

class StateEstimator_Impl : public controls::state::StateEstimator

Public Functions

StateEstimator_Impl(std::mutex &mutex, LoggerFunc logger)

Actual constructor, called implicitly by make_shared.

Parameters:

  • mutex – StateEstimator’s mutex

  • logger – logger function to be used

virtual void on_spline(const SplineMsg &spline_msg) override

Callback for spline messages. Updates the state estimator with the new spline. Used for both state projection and curvilinear state lookup table generation.

Parameters:

spline_msg – The spline message

virtual void on_twist(const TwistMsg &twist_msg, const rclcpp::Time &time) override

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) override

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 void render_and_sync(State state) override

“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 bool is_ready() override

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 void set_logger(LoggerFunc logger) override

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 void set_logger_obj(rclcpp::Logger logger) override

Attaches the node’s logger object to the state estimator.

Parameters:

logger – The logger object to be bound.

virtual void record_control_action(const Action &action, const rclcpp::Time &time) override

Records a control action for state projection purposes.

Parameters:

  • action – The control action to record

  • ros_time – The time the action was recorded

~StateEstimator_Impl() override

Destructor, cleans up OpenGL resources.

Private Functions

void gen_tex_info(glm::fvec2 car_pos)

Calculates scale and center transformation from world to lookup table, stores in m_curv_frame_lookup_tex_info.

void render_curv_frame_lookup()

Render the lookup table into m_curv_frame_lookup_fbo

void map_curv_frame_lookup()

Maps the rendered OpenGL frame buffer to a CUDA texture (cuda_globals::curv_frame_lookup_tex) Since the OpenGL frame buffer can’t be referred to by the rest of the code directly.

void unmap_curv_frame_lookup()

Unmaps the frame buffer from CUDA memory, since you can’t render to something when it is mapped. Think about it like releasing a mutex.

void sync_tex_info()

Syncs m_curv_frame_lookup_tex_info to cuda_globals so it can be accessed by MPPI.

void gen_curv_frame_lookup_framebuffer()

Creates the frame and render buffers (m_curv_frame_lookup_fbo/rbo).

void gen_gl_path(utils::GLObj &gl_path)

Creates the names for the vao, vbo and ebo. Specifies how the vbo should be laid out, stores this in the vao. Lastly, binds to the ebo.

Creates the buffers to be used, as well as the descriptions of how the buffers are laid out.

void fill_path_buffers_cones()

Fills in the vertex and element buffers with the actual vertex position information and the triples of vertex indices that represent triangles respectively.

Private Members

std::vector<glm::fvec2> m_spline_frames

Stores the sequence of (x,y) spline points from path planning.

state::StateProjector m_state_projector

The owned StateProjector to estimate the current state.

cuda_globals::CurvFrameLookupTexInfo m_curv_frame_lookup_tex_info

Center and scale information (transformation from IRL to rendered coordinates)

GLuint m_curv_frame_lookup_fbo

Frame buffer object. Where the OpenGL shader pipeline renders to. The actual lookup table. OpenGL object, not a CUDA object.

GLuint m_curv_frame_lookup_rbo

Render buffer object. Render target for the fbo, used to map to CUDA memory.

TODO:

  1. Make the fbo complete

  2. Check for completeness

  3. Delete the framebuffer after use

utils::GLObj m_gl_path

Has members vbo, vao, veo. utils::GLObj vbo - vertex buffer object. Contiguous block of memory where all the vertices’ information is stored vao - vertex array object. A specification for how memory in the VBO is laid out, e.g. how big is a vertex. ebo - element buffer object. Memory that contains triples of indices into the VBO, representing triangles.

GLuint m_curv_frame_lookup_shader_program

Shader program to be used. Composed of vertex and fragment shaders.

SDL_GLContext m_gl_context

OpenGL context, stores all the information associated with this instance.

SDL_Window *m_gl_window

OpenGL window assoicated with the context.

bool m_curv_frame_lookup_mapped = false

Whether the frame buffer is mapped to CUDA memory or not.

LoggerFunc m_logger

The logger function to be used.

rclcpp::Logger m_logger_obj

Logger object (belonging to the node)

std::mutex &m_mutex

Mutex to prevent multiple method calls from happening simultaneously.

Private Static Attributes

static GLint shader_scale_loc = 0

Index into where the uniform scale is stored for vertex shader to reference.

static GLint shader_center_loc = 1

Index into where the uniform center is stored for vertex shader to reference.

struct FakeTrackInfo
struct Vertex

Public Members

struct controls::state::StateEstimator_Impl::Vertex curv

Curvilinear coordinates. Progress = distance along spline, offset = perpendicular distance from spline, heading = angle relative to spline.