Add servoing

[NotNotQuinn]

Currently servoing is working.

Some notes:

• The servo node can accept joystick values itself, it seems the example config's scaling ( https://github.com/moveit/moveit2/blob/main/moveit_ros/moveit_servo/config/panda_simulated_config.yaml ) works better than the previously configured speed values in joystick.yaml. The speed is too high and it cannot find IK solutions. I'll keep the infrastructure in place for the setting the speed in case we ever add a plan/execute mode, but in the current state it's not in use.
• The servo node must be enabled!! Run ros2 service call /servo_node/start_servo std_srvs/srv/Trigger {} after starting the node otherwise it will silently do nothing. (how convenient)
• The gripper part of this hasn't been modified. I'm thinking it will be modified to send the CAN-PPM messages.
• The joystick must be plugged in before starting the docker container.
• Smoothing and collision checking are disabled, it seems more stable like this. Something is up with collision checking, I haven't done too much investigation but when it's enabled it will only move like +/- 7e-5 m with a warning about decelerating for collision and/or singularity. Likely self-collision.
• Importing a point cloud for obstacle avoidance is literally as easy as naming which topic to listen to (hopefully). -> although it is important to keep in mind the rover will be moving, and we will somehow need to account for that.
• The publish_period was heavily increased (reducing the rate), as when it was set to the example's value the IK for one servo command wouldn't finish before the next being sent, leading to no movement at all except for the final command when you stop command a movement.
• I forgot to adjust the axes in joystick.yaml before commiting, but currently the control is quite unintuitive (left -> go forward, I think)

The next things I think are important to focus on: (in different PRs)

• Convert the gripper controller to use the CAN-PPM messages (babbler)
• Automatically call the servo_start service. (maybe with a different enable/disable button)
• Fix the axes (just need to look at the documentation for game_controller_node)
• Foxglove extensions (or something) to show what commands are being sent.