Swerve Drive Controller

[VAM7686]

Description

Swerve drive motor controller.

This PR does the following:

• Adds a new swerve drive motor controller node.
• Fixes issues with motor direction in the swervi urdf.
• Adds unit tests for the new swerve drive motor controller node.
• Fixes minor issue with gazebo swerve drive control node.
• Tunes PID controller for swivels

Fixes #816

Testing steps

Note: All movements should be in accordance with REP 103.

Test Case 1

1. Run roslaunch igvc_gazebo ramp_lane.launch model:=1
2. Run roslaunch igvc_navigation swerve_drive.launch
3. Run

   rostopic pub /cmd_vel geometry_msgs/Twist "linear:
   x: 1.0
   y: 0.0
   z: 0.0
   angular:
   x: 0.0
   y: 0.0
   z: 0.0"

Expectation: Swervi should go directly forward.

Test Case 2

1. Run roslaunch igvc_gazebo ramp_lane.launch model:=1
2. Run roslaunch igvc_navigation swerve_drive.launch
3. Run

   rostopic pub /cmd_vel geometry_msgs/Twist "linear:
   x: 0.0
   y: 1.0
   z: 0.0
   angular:
   x: 0.0
   y: 0.0
   z: 0.0"

Expectation: Swervi should go to the left.

Test Case 3

1. Run roslaunch igvc_gazebo ramp_lane.launch model:=1
2. Run roslaunch igvc_navigation swerve_drive.launch
3. Run

   rostopic pub /cmd_vel geometry_msgs/Twist "linear:
   x: 0.0
   y: 0.0
   z: 0.0
   angular:
   x: 0.0
   y: 0.0
   z: 1.0"

Expectation: Swervi should rotate counter clockwise.

Test Case 4

1. Run catkin_make run_tests_igvc_platform_rostest_src_tests_test_test_swerve_control.test

Expectation: All tests pass.

Self Checklist

• [x] I have formatted my code using make format
• [x] I have tested that the new behaviour works