# Add key of ros repo
sudo apt-key adv --keyserver hkp://ha.pool.sks-keyservers.net:80 --recv-key 421C365BD9FF1F717815A3895523BAEEB01FA116
# Add ros repo
sudo sh -c 'echo "deb http://packages.ros.org/ros/ubuntu $(lsb_release -sc) main" > /etc/apt/sources.list.d/ros-latest.list'
# Install ros
sudo apt update
sudo apt install -y ros-kinetic-desktop-full
apt search ros-kinetic

# install ros dependency
sudo rosdep init
rosdep update

## OPTIONAL: Auto source ROS
echo "source /opt/ros/kinetic/setup.bash" >> ~/.bashrc && source ~/.bashrc

## OPTIONAL: Install python packages for ros
sudo apt install -y python-rosinstall python-rosinstall-generator python-wstool build-essential

# Create workspace
. /opt/ros/kinetic/setup.bash
mkdir -p catkin_ws/src
cd catkin_ws/src
catkin_init_workspace
cd ..
catkin_make

# Install RosAria
cd ~/
mkdir tmp
cd tmp
git clone https://github.com/reedhedges/AriaCoda
cd AriaCoda/src/
make
sudo make install

# Install ROS packages
sudo apt-get install -y ros-kinetic-teleop-twist-keyboard && sudo apt-get install -y ros-kinetic-joy

# Install Teleop-Joy package
cd ~/catkin_ws/src
git clone https://github.com/ros-teleop/teleop_twist_joy
cd ..
catkin_make

# Connect and test joystick
sudo jstest /dev/input/js0
# Move joystick

#### RUN EVERYTHING
# Terminal window 1: Roscore
roscore

# Terminal window 2: RosAria
cd ~/catkin_ws && . devel/setup.bash
sudo chmod a+rw /dev/ttyUSB0
rosrun rosaria RosAria

# Terminal window 3: Joystick
cd ~/catkin_ws_. 
. devel/setup.bash
rosrun joy joy_node _autorepeat_rate:=10

# Terminal window 4: Joystick to RosAria connection
cd ~/catkin_ws
. devel/setup.bash
rosparam set joy_node/dev "dev/input/js0"
rosrun teleop_twist_joy teleop_node cmd_vel:=RosAria/cmd_vel _scale_linear:=0.2 _scale_angular:=0.5 _axis_linear:=5 _axis_angular:=4

## OPTIONAL: Terminal window 5: Check graph
rqt_graph

####### Kinect

rosrun kinect2_bridge kinect2_bridge _reg_method:=cpu