Make a state_maintainer node which can be launched in the pipeline and maintain a state

[loganfillo]

We need a component node which that is responsible for maintaining the current state of the AUV when it is launched in the pipeline. This node should be a relatively simple C++ component node in the package triton_controls called state_maintainer. It should subscribe to /triton/state which publishes geometry_msgs::msg::Pose messages. It should publish to /triton/controls/input_pose also using geometry_msgs::msg::Pose messages. The node should have parameters which indicate an offset from the state the AUV was in when the node was first loaded. The node should use the adjusted pose or the current pose as the message that it publishes to the controller, depending on whether or not the offset parameters are given.

Another idea is to make the node have a service which can be used to adjust the state it is meant to control.

Task

• [x] Create the component node state_maintainer in triton_controls

• [x] Add parameters for pose offset

• [x] Set up the communication between the required topics and ensure they work

• [ ] Either add a service to change the state to maintain, or learn how to use the SetParameter servie in rcl_interfaces and setup a parameter callback

[Over-TheRainbow]

I was able to set the pose the node uses using the config yaml, and convert the roll pitch yaw into a quaternion. Is there anything else I need to do or should I just start cleaning this up?

[loganfillo]

I was able to set the pose the node uses using the config yaml, and convert the roll pitch yaw into a quaternion. Is there anything else I need to do or should I just start cleaning this up?

@Over-TheRainbow The next step would be getting it to run in the pipeline, this would be easy for the simple no-offset case, but the case where we need to add parameters if we want an offset will be tricky. Try looking into the LoadNode service becuase I know it has a parameters argument so that you can load a node with parameters. We will have to adjust the pipeline config file format a bit to allow for passing parameters in the config which will be passed to LoadNode