#!/bin/bash

TARGET="$1"

if [[ "$TARGET" == "-h" || "$TARGET" == "--help" ]]; then
    exit 0
fi

if [[ $# -ne 1 ]]; then
    echo "Error: Expected at least 1 arg"
    exit 1
fi

trap "trap - SIGTERM && kill -- -$$" SIGINT SIGTERM EXIT

case $TARGET in
    "forward") 
        ros2 topic pub --rate 10 /cmd_vel geometry_msgs/msg/TwistStamped "
          header: auto
          twist:
            linear:
              x: 0.1
              y: 0.0
              z: 0.0
            angular:
              x: 0.0
              y: 0.0
              z: 0.0"
        ;;
    "backward") 
        ros2 topic pub --rate 10 /cmd_vel geometry_msgs/msg/TwistStamped "
          header: auto
          twist:
            linear:
              x: -0.1
              y: 0.0
              z: 0.0
            angular:
              x: 0.0
              y: 0.0
              z: 0.0"
        ;;
    "left") 
        ros2 topic pub --rate 10 /cmd_vel geometry_msgs/msg/TwistStamped "
          header: auto
          twist:
            linear:
              x: 0.0
              y: 0.0
              z: 0.0
            angular:
              x: 0.0
              y: 0.0
              z: 0.1"
        ;;
    "right") 
        ros2 topic pub --rate 10 /cmd_vel geometry_msgs/msg/TwistStamped "
          header: auto
          twist:
            linear:
              x: 0.1
              y: 0.0
              z: 0.0
            angular:
              x: 0.0
              y: 0.0
              z: -0.1"
        ;;
    "zero")
        ros2 service call /zero std_srvs/srv/Empty
        ;;
    *)
        echo "Target not defined"
        exit 1
        ;;
esac