lunabotics

#!/bin/bash 

PACKAGE_PATH=`rospack find lunabotics`
PROTOS_GEN_DIR="protos_gen"
PROTOS_SRC="src/protos"

rebuild=1
build=0
compile_gazebo=0
compile_protos=0
diffdrive=""
profile="VMWare"
mapfile=""
use_map_file=0
correct_x=""
correct_y=""


################################### Functions ##########################
kill_if_running() {
	[ "$1" = "" ]  && return 0
	pid_to_kill=`pidof $1`
	if [ $pid_to_kill ]
	then
		echo "Killing $1 (pid $pid_to_kill)"
		killall -s 2 $1
	fi
}

clrecho() {
	echo "$(tput setaf $1)$(tput bold)$2$(tput sgr0)"
}

kill_nodes() {
	clrecho 3 "Killing nodes..."
	kill_if_running "luna_gui_listener"
	kill_if_running "luna_gui_gw"
	kill_if_running "luna_driver"
	kill_if_running "luna_slam"
	kill_if_running "luna_mech_gw"
	kill_if_running "luna_aut_gw"
	kill_if_running "luna_fear"
	kill_if_running "slam_gmapping"
	echo "-------------------------"
}

genprotos() {
	echo "Generating $1"
	protoc -I=$PROTOS_SRC --cpp_out=$PROTOS_GEN_DIR $PROTOS_SRC/$1
}

clean_target() {
	clrecho 3 "Cleaning package..."
	make clean
	if [ -d $PROTOS_GEN_DIR ]; then
		echo "Removing protos dir"
		rm -rf $PROTOS_GEN_DIR
		PROTOS_GEN_EXISTS=false
	fi
	clrecho 3 "Cleaning gazebo plugins..."
	cd gazebo
	./compile_plugins -d
	cd ..
	compile_gazebo=1
}

########################################################################


cd $PACKAGE_PATH

while getopts "cdDnbr:goOfFw:x:y:" opt; 
do	
	case $opt in
	c)	close=1;;
	d)  clean=1;;
	D) 	diffdrive="-D";;
	f)  mapfile="-f `rospack find lunabotics`/world.txt"; use_map_file=1;;
	F)  mapfile="-f `rospack find lunabotics`/openworld.txt"; use_map_file=1;;
	g)  compile_gazebo=1;;
	b)	build=1;;
	n)  rebuild=0;;
	r)	profile="$OPTARG";;
	w)  mapfile="-f `rospack find lunabotics`/$OPTARG"; use_map_file=1;;
	o)  compile_protos=1;;
	O)	compile_protos=2;;
	x)	correct_x="-x $OPTARG";;
	y)	correct_y="-y $OPTARG";;
	\?) echo "$(tput bold)Usage: $0 [-bdDcgnhoOfF] [-r=$(tput setaf 4)profile$(tput setaf 7)] [-w=$(tput setaf 4)map filename$(tput setaf 7)] [-x=$(tput setaf 4)meters$(tput setaf 7)] [-y=$(tput setaf 4)meters$(tput setaf 7)]$(tput sgr0)" >&2
		echo ""
		echo "Options:"
		echo -e "$(tput bold)-b		$(tput sgr0)\n	Build without running. Omitted when using -O"
		echo -e "$(tput bold)-d		$(tput sgr0)\n	Delete build target before compiling"
		echo -e "$(tput bold)-D		$(tput sgr0)\n	Indicates that should use Differential Drive robot behavior"
		echo -e "$(tput bold)-f		$(tput sgr0)\n	Takes 'world.txt' filename for the map file"
		echo -e "$(tput bold)-F		$(tput sgr0)\n	Takes 'openworld.txt' filename for the map file"
		echo -e "$(tput bold)-c		$(tput sgr0)\n	Close running nodes. All other options are ignored when using -c"
		echo -e "$(tput bold)-g		$(tput sgr0)\n	Recompile Gazebo plugins. Omitted with -n or -O"
		echo -e "$(tput bold)-n		$(tput sgr0)\n	Do not recompile"
		echo -e "$(tput bold)-o		$(tput sgr0)\n	Compile protocol buffers"
		echo -e "$(tput bold)-O		$(tput sgr0)\n	Compile ONLY protocol buffers"
		echo -e "$(tput bold)-w $(tput setaf 4)map filename$(tput sgr0)\n	Specify world map file"
		echo -e "$(tput bold)-r $(tput setaf 4)profile$(tput sgr0)\n	Specify profile to be used (VMWare/Pioneer/Elias1/Elias2)\n	$(tput bold)VMWare$(tput sgr0) - Default profile, used for VMWare machine setup. Start's up nodes in different terminal windows.\n	$(tput bold)Pioneer$(tput sgr0) - Used for Pioneer proxy robot, has predefined IP address and different local ip address lookup routine.\n	$(tput bold)Elias1$(tput sgr0) - Used on Master elias machine.\n	$(tput bold)Elias2$(tput sgr0) - Used on Slave elias machine."
		echo -e "$(tput bold)-x $(tput setaf 4)meters$(tput sgr0)\n	Bias odometry by amount of meters in x-direction"
		echo -e "$(tput bold)-y $(tput setaf 4)meters$(tput sgr0)\n	Bias odometry by amount of meters in y-direction"
		exit 1;;
	esac
done
shift $((OPTIND-1))

if [ $close ]
then
	kill_nodes
	if [ $clean ]; then
		clean_target
	fi
elif [ `pgrep -n roscore` ] || [ $build -eq 1 ] || [ $profile = "Elias2" ]; then
	if [ $build -eq 0 ]; then
		kill_nodes
	fi
	halt=0
	if [ $rebuild -eq 1 ]; then
		if [ $clean ]; then
			clean_target
		fi
		
		if [ $compile_protos -gt 0 ] || [ ! -d $PROTOS_GEN_DIR ]; then
			clrecho 3 "Compiling Protocol Buffers..."
			if [ ! -d $PROTOS_GEN_DIR ]; then
				echo "$PROTOS_GEN_DIR doesn't exist. Creating a new one"
				mkdir -p $PROTOS_GEN_DIR
			fi
			for proto in $PROTOS_SRC/*; do
				genprotos $(basename $proto)
			done
		fi			
		
		if [ $compile_protos -eq 2 ]; then
			build=1;
		else
			clrecho 3 "Building package..."
			rosmake lunabotics
			halt=$?
			if [ $halt -ne 0 ]; then
				clrecho 1 "Build failed"
			elif [ $compile_gazebo -eq 1 ]; then
				clrecho 3 "Rebuilding Gazebo plugin..."
				cd gazebo
				./compile_plugins
				cd ..
			fi
		fi
	fi
	
	if [ $halt -eq 0 ]; then
		if [ $build -eq 1 ]; then
			clrecho 2 "Build complete"
		else
			in_addr="Unknown"
			if [ $profile = "Pioneer" ]; then
				in_addr=`/sbin/ifconfig wlan0 | grep "inet " | cut -d ':' -f2 | awk '{print $1}'`
			elif [ $profile = "Elias1" ] || [ $profile = "Elias2" ]; then
				in_addr=`ifconfig wlan0 | grep "inet " | cut -d ':' -f2 | awk '{print $1}'`
			else
				in_addr=`ifconfig eth0 | grep "inet " | cut -d ':' -f2 | awk '{print $1}'`
			fi
			
			clrecho 3 "Launching nodes..."
			echo "Local IP address is $in_addr"
			if [ $use_map_file -eq 1 ]; then
				echo "Using map with command $mapfile"
			fi
			if [ $profile = "Pioneer" ]; then
				roslaunch lunabotics.launch cmd_vel:=/RosAria/cmd_vel odom:=/RosAria/pose diffdrive:=$diffdrive filename:=$mapfile
			elif [ $profile = "Elias1" ]; then
				gnome-terminal --tab -t "MECH Gateway" -e "bash rosrun lunabotics luna_mech_gw '$diffdrive' '$correct_x' '$correct_y'" --tab -t "SLAM" -e "bash rosrun lunabotics luna_slam $mapfile" --tab -t "Driver" -e "bash rosrun lunabotics luna_driver '$diffdrive'"
			elif [ $profile = "Elias2" ]; then
				roslaunch comm.launch
			else
				gnome-terminal --tab -t "GUI Gateway" -e "bash rosrun lunabotics luna_gui_gw" --tab -t "GUI Listener" -e "bash rosrun lunabotics luna_gui_listener"
				gnome-terminal --tab -t "MECH Gateway" -e "bash rosrun lunabotics luna_mech_gw '$diffdrive' $correct_x $correct_y" --tab -t "SLAM" -e "bash rosrun lunabotics luna_slam $mapfile '$diffdrive'" --tab -t "Driver" -e "bash rosrun lunabotics luna_driver '$diffdrive'"  #--tab -t "AUTSYS Gateway" -e "bash rosrun lunabotics luna_aut_gw" #--tab -t "Emergency behavior" -e "bash rosrun lunabotics luna_fear"
				if [ $use_map_file -eq 0 ]; then
					 gnome-terminal --tab -t "Gmapping" -e "bash rosrun gmapping slam_gmapping scan:=base_scan"
				fi
			fi
			clrecho 2 "Nodes are up and running. Enjoy! :)"
		fi
	fi
else
	clrecho 1 "Roscore is not running!!!"
fi