Merge pull request #64 from Team766/betterLocations

Changes for new Location Tracking

Installer/AprilTags/ApriltagsManager.sh → Installer/AprilTags/AprilTagsManager.sh

@@ -5,13 +5,13 @@ if [ $# -ne 1 ]; then
     exit 1
 fi
 
-if [ ! -f '/apps/AprilTags/Backend/ws_server' ]; then
+if [ ! -f '/opt/AprilTags/Backend/ws_server' ]; then
     echo "missing backend"
     exit 2
 fi
 
 function killIfRunning() {
-    PID=`libAprilTags.sh pid $1`
+    PID=`libAprilTags pid $1`
     if [ $PID -ne 0 ]; then
         kill -2 $PID
         return 1
@@ -20,23 +20,20 @@ function killIfRunning() {
 }
 
 # set clocks to max frequency (jetson only)
-jetson_clocks || true
+jetson_clocks || true 1>&2 >> /dev/null
 
 #open source for args
-source /apps/AprilTags/args
-
-# make sure that the bin dir is actually loaded
-PATH="$PATH:/apps/bin"
+source /opt/AprilTags/args
 
 if [[ $1 == "start" ]]; then
     # check for lock
-    if [ -f "/apps/AprilTags/servicerunning" ]; then
+    if [ -f "/opt/AprilTags/servicerunning" ]; then
         echo "service is already running"
-        echo "HINT: if you think the service is not running, then run 'AprilTags.sh --validate lockfile'"
+        echo "HINT: if you think the service is not running, then run 'AprilTags --validate lockfile'"
         # now check if it is allowed to remove the lockfile..
         if [[ $autormlockfile == "true" ]]; then
         	# check for if the services are fine
-        	status=$(/apps/bin/AprilTags.sh -V lockfile)
+        	status=$(AprilTags -V lockfile)
         	lockfilestatus=$(echo $status | awk -F ";" '{print $1}' | awk -F "=" '{print $2}')
         	if [[ $lockfilestatus == "false" ]]; then
         		# the lockfile is gone, and nothing is wrong
@@ -52,62 +49,70 @@ if [[ $1 == "start" ]]; then
         fi
     fi
 
-    # to fix some odd pathing things
-    cd /apps/AprilTags
-
-    touch /apps/AprilTags/servicerunning
+    touch /opt/AprilTags/servicerunning
 
-    cams=`libAprilTags.sh camIDs`
+    cams=`libAprilTags camIDs`
     if [[ $? -ne 0 ]]; then
     	echo "No cameras found or something. Try plugging some in, or else the cameras are not being seen."
     	exit 5
     fi
 
-    # cd again just to be safe (that lib WILL cd)
-    cd /apps/AprilTags
+    cd /opt/AprilTags # seasocks depends on this
 
     # iterate
     for cam in $cams; do
         # set items off of the output cam
         IFS=: read -r camID camIndex <<< "$cam"
-
-        # not yet
+
         # get the offset file
-        #camLoc=$(libAprilTags.sh getCamLoc $camID)
-        #if [[ $? -ne 0 ]]; then
-        #    echo "FAILED TO FIND CAMERA OFFSET ${camID}, EXITING..."
-        #    exit 6
-        #fi
+        camLoc=$(libAprilTags getCamLoc $camID)
+        if [[ $? -ne 0 ]]; then
+            echo "FAILED TO FIND CAMERA OFFSET ${camID}, EXITING..."
+            exit 6
+        fi
+
+        camFilePlace="/opt/AprilTags/data/camlocs/cam-${camLoc}-offset.json"
+        if ! [[ -f $camFilePlace ]]; then
+            echo "FAILED TO FIND CAMERA OFFSET FILE FOR CAM ${camID} with location ${camLoc} (file missing ${camFilePlace}"
+            exit 6
+        fi
 
         # ensure that the calibration file exists
-        if ! [[ -f /apps/AprilTags/data/calibration/calibrationmatrix_${camID}.json ]]; then
+        if ! [[ -f /opt/AprilTags/data/calibration/calibrationmatrix_${camID}.json ]]; then
             echo "FAILED TO FIND CAMERA CALIBRATION FILE ${camID}, EXITING..."
             exit 6
         fi
 
+        # set camName
+        if [[ ${camLoc} == "TEMPLATE" ]]; then
+            camName="cam${camID}"
+        else
+            camName="${camLoc}"
+        fi
+
         # set args
         args=`$backend` # this actually is magical because of the weird eval thing
         # https://stackoverflow.com/questions/5112663/bash-variable-reevaluation
 
         # abs path BECAUSE of the proc getting commands
-        /apps/AprilTags/Backend/ws_server $args &
-        echo $! >> /apps/AprilTags/servicerunning # add every pid to a new line
+        /opt/AprilTags/Backend/ws_server $args &
+        echo $! >> /opt/AprilTags/servicerunning # add every pid to a new line
     done
 
     exit
 
 elif [[ $1 == "stop" ]]; then
-    if [[ -f /apps/AprilTags/servicerunning ]]; then
+    if [[ -f /opt/AprilTags/servicerunning ]]; then
     	# read the file, kill if a pid is found inside
     	while read p; do
     	    if [[ "$p" =~ ^-?[0-9]+$ ]]; then # regex to find int
     	    	# assume its a PID that belongs to ws_server if running
     	    	if ps -p $p > /dev/null
     	    	then
-    	    	    timeout -s INT 5s 'killHandler.sh ${p}' &
+    	    	    timeout -s INT 5s 'libAprilTags killHandler ${p}' &
     	        fi
     	    fi
-    	done < /apps/AprilTags/servicerunning
+    	done < /opt/AprilTags/servicerunning
     fi
     # and add the rest of it
     # this is broken, and I am too lazy to fix it
@@ -116,7 +121,7 @@ elif [[ $1 == "stop" ]]; then
     #	continue # just run the initial condition, a backup if the service orphaned something
     #done
 
-    rm /apps/AprilTags/servicerunning
+    rm /opt/AprilTags/servicerunning
 else
     echo "input ${1} not understood"
     exit 3

Installer/AprilTags/args

@@ -1,6 +1,6 @@
 # Please be careful editing this file!
 # It looks odd and it is structured in two seperate ways, but it is fine, write inside the echo for backend
-# ${camIndex} is the video index of the camera, ${camID} is the id of the camera (by serial), and ${camLoc} is the location of the camera on the robot (as in tr for example to be top right)
-backend='eval echo "-camera_idx ${camIndex} -cal_file /apps/AprilTags/data/calibration/calibrationmatrix_${camID}.json -camera_name cam${camID}"'
+# ${camIndex} is the video index of the camera, ${camID} is the id of the camera (by serial), and ${camLoc} is the location of the camera on the robot (as in tr for example to be top right), ${camName} is the name of the camera
+backend='eval echo "-camera_idx ${camIndex} -cal_file /opt/AprilTags/data/calibration/calibrationmatrix_${camID}.json -position_file ${camFilePlace} -camera_name ${camName}"'
 # manager
 autormlockfile="true"

Installer/AprilTagsPipeline.service

@@ -4,9 +4,9 @@ Description=Start/teardown apriltags stuff
 
 [Service]
 Type=oneshot
-ExecStart=/apps/AprilTags/ApriltagsManager.sh start
+ExecStart=/opt/AprilTags/AprilTagsManager.sh start
 RemainAfterExit=true
-ExecStop=/apps/AprilTags/ApriltagsManager.sh stop
+ExecStop=/opt/AprilTags/ApriltagsManager.sh stop
 StandardOutput=journal
 #Restart=always
 

Installer/bin/AprilTags.sh

@@ -13,6 +13,8 @@ function help() {
     exit 0
 }
 
+VERSION="1.4 Better Locations (Grape Juice)" # I think having codenames for version could be fun
+
 if [ $# -eq 0 ]; then
     # print help
     help
@@ -61,6 +63,10 @@ while [[ $# -gt 0 ]]; do
       update="t"
       shift # past arguement
       ;;
+    --version)
+      echo $VERSION
+      exit 0
+      ;;
     *|-*|--*)
       echo "Unknown option $1"
       exit 1
@@ -73,16 +79,16 @@ if [ ! -z ${valitem+x} ]; then
     printV "validating arg $valitem..."
     if [[ $valitem == "lockfile" ]]; then
         # check for the existance of the lock
-        PID=$(libAprilTags.sh pid "/apps/AprilTags/Backend/ws_server")
+        PID=$(libAprilTags pid "/opt/AprilTags/Backend/ws_server")
         printV "BackendPID is $backPID. (zero is not running)"
-        if [ -f /apps/AprilTags/servicerunning ]; then
+        if [ -f /opt/AprilTags/servicerunning ]; then
             # lockfile is there
             printV "lockfile present, checking if backend is being run"
             if [ $PID -ne 0 ]; then
                 echo "lockfile=true;running=true;"
                 printV "The service appears to be running and stable."
             else
-                rm /apps/AprilTags/servicerunning
+                rm /opt/AprilTags/servicerunning
                 if [ $? -ne 0 ]; then
                     echo "lockfile=rm;running=false;"
                     printV "Lockfile found with the service not running, it could not be deleted, feel free to delete it."
@@ -99,7 +105,7 @@ if [ ! -z ${valitem+x} ]; then
                 printV "The AprilTags service is offline and the lockfile is not there."
             else
                 printV "The lockfile is missing and at least one of the services is running."
-                touch /apps/AprilTags/servicerunning
+                touch /opt/AprilTags/servicerunning
                 if [ $? -ne 0 ]; then
                     echo "lockfile=add;running=true;"
                     printV "The lockfile couldn't be created. please make it, or stop the processes!"
@@ -127,34 +133,33 @@ elif [ $update == "t" ]; then
     # disable the service to make sure stuff is ok
     systemctl stop AprilTagsPipeline.service
     # just as a check, make sure that the lockfile is also gone
-    rm /apps/AprilTags/servicerunning
+    rm /opt/AprilTags/servicerunning
 
     # make sure that the dirs exist. why did I not do this before
-    if [ ! -d /apps/AprilTags/Backend ]; then
-    	mkdir /apps/AprilTags/Backend
+    if [ ! -d /opt/AprilTags/Backend ]; then
+    	mkdir /opt/AprilTags/Backend
     fi
 
     printV "copying backend..."
-    cp -R build/* /apps/AprilTags/Backend/
+    cp -R build/* /opt/AprilTags/Backend/
 
     printV "copying data..."
     # remove the existing data dir if it exists then replace it
-    if [ -d /apps/AprilTags/data ]; then
-    	rm -rf /apps/AprilTags/data
+    if [ -d /opt/AprilTags/data ]; then
+    	rm -rf /opt/AprilTags/data
     fi
     if [ -d data ]; then
-    	cp -R data/ /apps/AprilTags/data/
+    	cp -R data/ /opt/AprilTags/data/
     fi
 
     printV "copying public server files..."
     if [ -d public ]; then
-        cp -R public/ /apps/AprilTags/public
+        cp -R public/ /opt/AprilTags/public
     fi
 
     printV "The files were sucessfully coppied"
 
     # restart the service
     systemctl start AprilTagsPipeline.service
-
 # end of the args and stuffs
 fi

Installer/bin/libAprilTags.sh

@@ -5,7 +5,7 @@
 # get proc pid (0 notfound/error, else is the pid)
 function getProcPID() {
     # NOTE: this returns the OLDEST PID
-    paths=$(ps -aux | grep $1 | grep -v 'grep' | grep -v 'libAprilTags.sh pid') #last section removes command syntax for this command
+    paths=$(ps -aux | grep $1 | grep -v 'grep' | grep -v 'libAprilTags pid') #last section removes command syntax for this command
     # the quotes preserve linebreaks
     # rewriting the for loop, since its done in a subshell and causes issues when written normally
     while read line; do
@@ -24,20 +24,10 @@ echoerr() { echo "$@" 1>&2; } # bypass capture by var assigning because std::err
 if [[ $1 == "pid" ]]; then
     getProcPID $2
 elif [[ $1 == "camIDs" ]]; then
-    cd "${0%/*}"/camerascanner
-    # run the go file in the most efficent way possible
-    arch=$(uname -m)
-    if [[ -f "scanner_${arch}" ]]; then
-        ret=$("./scanner_${arch}")
+    if [[ -f /bin/AprilTags_camerascanner ]]; then
+        ret=$(AprilTags_camerascanner)
     else
-    	if [[ $arch == "x86_64" ]]; then
-    	    # x86 can run aarch64 binaries for some reason, so do that
-    	    ret=$("./scanner_aarch64")
-
-    	else
-            echoerr "running go file directly"
-            ret=$(go run main)
-        fi
+        ret=$(go run /opt/AprilTags/bin/camerascanner/scanner.go)
     fi
 
     if [[ "${ret,,}" == *"err"* ]] || [[ "${ret,,}" == *"fault"* ]]; then
@@ -53,21 +43,46 @@ elif [[ $1 == "camIDs" ]]; then
     echo "${ret:1: -1}"
     exit 0
 elif [[ $1 == "getCamLoc" ]]; then
-    camlocfile="/apps/AprilTags/data/cameralocations"
+    camlocfile="/opt/AprilTags/data/camlocations"
     camid=$2
-    if ! [[ -f camlocfile ]]; then
+    if ! [[ -f $camlocfile ]]; then
         echo "cam locations file not found."
         exit 1 # maybe stop using the same number
     fi
     # must have the file, assume that the file is correct
     set -e # just in case
-    locfile=$(tail -n +1 camlocfile)
-    for line in locfile; do
-        if [[ "$2" == "${line[0]}" ]]; then
-            echo line[1]
-            exit 0
+    found=false
+    cat $camlocfile | while read line || [ -n "$line" ] ; do
+        lineparts=($line)
+        if [[ "$2" == "${lineparts[0]}" ]] && [[ "$2" != "id" ]]; then
+            echo ${lineparts[1]}
+            found=true
+            # exit does nothing
         fi
     done
-    echo "id not found"
+    if [[ found ]]; then
+        exit 0
+    fi
+    echoerr "id not found"
     exit 1 # again, same code
+elif [[ $1 == "killHandler" ]]; then
+    if [[ "$2" =~ ^-?[0-9]+$ ]]; then # regex to find int
+        # assume its a PID that belongs to ws_server if running
+        if ps -p $2 > /dev/null
+        then
+            killpid=$2
+        fi
+    fi
+
+    # stop if unset
+    if [ -z ${killpid+x} ]; then exit 1; fi
+
+    # catch: kill -9
+    trap 'kill -9 ${killpid}' INT
+
+    kill -2 ${killpid}
+
+    while [ -d "/proc/$killpid" ]; do
+        sleep 0.1 $ wait $!
+    done
 fi