Create waypoint_sequence node and add service to waypoint_marker

[loganfillo]

The waypoint_marker should have a service that can be called and is used to the waypoint. It should have (Pose (x,y,z,r,p,y), Waypoint Type, Threshold for distance, Threshold for Rotation) and you will have to make a service type in triton_interfaces (https://docs.ros.org/en/rolling/Tutorials/Custom-ROS2-Interfaces.html). The waypoint marker should have two states, one for "waiting for waypoint", and the other is "pursuing waypoint". If it's waiting for waypoint, it isn't publishing control input, but if it's pursuing waypoint, it is publishing the control input, and also comparing state to the waypoint (i.e pose with threshold) and if it calculates that it has reached it's waypoint (different based on the type), then it will publish a boolean message to waypoint/feedback meaning that the last waypoint pursued was successfully reached.

The waypoint_sequence_manager should be a component node that can be configured with a yaml that is a list of params for the waypoint service. The yaml should look something like this

waypoint_sequence_manager:
  ros__parameters:
    num_waypoints: 2
    waypoint_1:
      pose:
        x: 1
        y: 0
        z: 0
        roll: 0
        pitch: 0
        yaw: 0
      type: stabilize
      threshold:
        position: 0.5
        rotation: 0.2
    waypoint_2:
      pose:
        x: 0
        y: 0
        z: 0
        roll: 3.14
        pitch: 0
        yaw: 0
      type: stabilize
      threshold:
        position: 0.5
        rotation: 0.2
    

This node will call the waypoint_marker's service to set the first waypoint, then wait until it receives success:true on waypoint/feedback before calling the service again with the next waypoint. Once it receives a success message for the last waypoint, it sends a success message to waypoint feedback. Below is an example of what a pipeline type config yaml will look like

# triangle.yml
/triton/pipeline_manager:
  ros__parameters:
    pipeline:
      components:
        - triton_controls::WaypointMarker
        - triton_controls::WaypointSequenceManager
      pkg_names: 
        - triton_controls
        - triton_controls
      param_files:
        - ''
        - triangle_waypoints.yaml
      namespace: /triton
    use_sim_time: false

Where triangle_waypoints.yaml will look like the format above

[Over-TheRainbow]

Notes:
2 states of Waypoint Marker
waiting for message to set input
broadcasting input pose

Waypoint Sequence
feed waypoint marker one by one from yaml list
notify Pipeline manager when list is complete
we can use this to test subbot to move in simple shapes