Added the ability to switch between cmd_vel control and induvidual motor control

scripts/test_wheel.sh

@@ -17,8 +17,6 @@ fi
 
 source install/setup.bash
 
-trap "trap - SIGTERM && kill -- -$$" SIGINT SIGTERM EXIT
-
 case $TARGET in
     "forward") 
         ros2 topic pub --rate 10 /cmd_vel geometry_msgs/msg/TwistStamped "
@@ -75,8 +73,19 @@ case $TARGET in
     "zero")
         ros2 service call /zero std_srvs/srv/Empty
         ;;
+    "vel")
+        ros2 topic pub --rate 10 /velocity_controller/commands std_msgs/msg/Float64MultiArray "
+          data: [$2, $3]"
+        ;;
+    "switch")
+        controllers=$(ros2 control list_controllers | grep -E "velocity|diffdrive")
+        to_deactivate=$(echo "$controllers" | awk '$3 == "active" { print $1 }')
+        to_activate=$(echo "$controllers" | awk '$3 == "inactive" { print $1 }')
+        ros2 control switch_controllers --deactivate $to_deactivate --activate $to_activate --best-effort
+        ;;
     *)
         echo "Target not defined"
         exit 1
         ;;
 esac
+exit 0