#!/bin/bash
# Copyright (C) 2022, Raffaello Bonghi <raffaello@rnext.it>
# All rights reserved
# Redistribution and use in source and binary forms, with or without
# modification, are permitted provided that the following conditions are met:
# 1. Redistributions of source code must retain the above copyright 
#    notice, this list of conditions and the following disclaimer.
# 2. Redistributions in binary form must reproduce the above copyright
#    notice, this list of conditions and the following disclaimer in the
#    documentation and/or other materials provided with the distribution.
# 3. Neither the name of the copyright holder nor the names of its 
#    contributors may be used to endorse or promote products derived 
#    from this software without specific prior written permission.
# 
# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND 
# CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, 
# BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 
# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
# HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 
# SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
# PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; 
# OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, 
# WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE 
# OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, 
# EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.

NANOSAUR_DATA='/opt/nanosaur'
NANOSAUR_VERSION='2.1.0'

# Variable stored in configuration file
ROS2_PATH='/opt/ros/foxy/setup.bash'
ROS_DEV_WS_NAME='ros_ws'
ROS_CORE_WS_NAME='nanosaur_core'
ROS_PERC_WS_NAME='nanosaur_perception'
DOCKER_TAG='latest'
PERCEPTION_DOCKER_TAG='latest'
BRANCH_TAG=$NANOSAUR_VERSION #Same tag release by default
NANOSAUR_COVER_TYPE='fisheye'
DOCKER_NETWORK='host'
RMW_IMPLEMENTATION='rmw_cyclonedds_cpp'
IGNITION_VERSION=fortress
SIMULATOR_INSTALLED=false
NANOSAUR_SIMULATION="ignition" # Default simulation
# Load nanosaur configuration file
if [ -f $NANOSAUR_DATA/.env ] ; then
    source $NANOSAUR_DATA/.env
fi

# Default variable
NANOSAUR_L4T="32.7"
ROBOT_NAME='nanosaur'
ROBOT_FILE_PATH=$NANOSAUR_DATA/'param/robot.yml'
NANOSAUR_DEV_WS_PATH=$HOME/$ROS_DEV_WS_NAME
NANOSAUR_CORE_WS_PATH=$HOME/$ROS_CORE_WS_NAME
NANOSAUR_PERCEPTION_WS_PATH=$HOME/$ROS_PERC_WS_NAME
ISAAC_ROS_WS_PATH=$HOME/isaac_ros_ws
# nanosaur keyboard command
nanosaur_keyboard_cmd="ros2 run teleop_twist_keyboard teleop_twist_keyboard --ros-args --remap cmd_vel:=/nanosaur/key_vel"

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

bold=`tput bold`
red=`tput setaf 1`
green=`tput setaf 2`
yellow=`tput setaf 3`
blue=`tput setaf 4`
reset=`tput sgr0`

# Load platform
# - aarch64 = NVIDIA Jetson
# - x86_64 = Desktop
# PLATFORM="$(uname -m)"
PLATFORM="desktop"
if [ -f /etc/nv_tegra_release ] ; then
    PLATFORM="robot"
fi

# Modules check sudo
# https://serverfault.com/questions/266039/temporarily-increasing-sudos-timeout-for-the-duration-of-an-install-script
sudo_me()
{
    local start=$1
    # write sudo me file
    local sudo_stat="/tmp/nanosaur_sudo_status"
    # No sudo loop
    if $start ; then
        touch $sudo_stat
        # Loop script
        while [ -f $sudo_stat ]; do
            # echo "checking $$ ...$(date)"
            sudo -v
            sleep 5
        done &
    else
        if [ -f $sudo_stat ] ; then
            rm $sudo_stat
        fi
    fi
}

get_nanosaur_rosinstall()
{
    local branch=$BRANCH_TAG
    # Load rosinstall configuration
    local ROSINSTALL_FILE="https://raw.githubusercontent.com/rnanosaur/nanosaur/$branch/nanosaur/rosinstall/desktop.rosinstall"
    if [[ $PLATFORM = "robot" ]] ; then
        ROSINSTALL_FILE="https://raw.githubusercontent.com/rnanosaur/nanosaur/$branch/nanosaur/rosinstall/robot.rosinstall"
    fi
    if $SIMULATOR_INSTALLED ; then
        ROSINSTALL_FILE="https://raw.githubusercontent.com/rnanosaur/nanosaur/$branch/nanosaur/rosinstall/simulations.rosinstall"
    fi
    echo $ROSINSTALL_FILE
}

get_perception_rosinstall()
{
    local branch=$BRANCH_TAG
    # Load rosinstall configuration
    echo "https://raw.githubusercontent.com/rnanosaur/nanosaur_perception/$branch/nanosaur_perception/rosinstall/perception.rosinstall"
}

############################ CONFIG functions #################################################

config_info()
{
    echo " - ${bold}Branch:${reset} ${green}$BRANCH_TAG${reset}" >&2
    echo " - ${bold}Cover type:${reset} ${green}$NANOSAUR_COVER_TYPE${reset}" >&2
    echo " - ${bold}DDS impementation:${reset} ${green}$RMW_IMPLEMENTATION${reset}" >&2
    if [[ $PLATFORM = "robot" ]] ; then
        echo " - ${bold}Network:${reset} ${green}$DOCKER_NETWORK${reset}" >&2
        echo " - ${bold}Tag:${reset} ${green}$DOCKER_TAG${reset}" >&2
    fi
    if [ ! -z ${ROS_DOMAIN_ID+x} ] ; then
        echo " - ${bold}ROS_DOMAIN_ID:${reset} ${green}$ROS_DOMAIN_ID${reset}" >&2
    else
        echo " - ${bold}ROS_DOMAIN_ID:${reset} 0 ${yellow}(not set)${reset}" >&2
    fi
    if $SIMULATOR_INSTALLED ; then
        echo "${bold}Simulation info:${reset}"
        echo " - ${bold}Default simulator:${reset} ${green}$NANOSAUR_SIMULATION${reset}" >&2
        echo " - ${bold}Ignition Gazebo version:${reset} ${green}$IGNITION_VERSION${reset}" >&2
    fi
}

config_save()
{
    # https://docs.docker.com/compose/compose-file/compose-file-v3/#variable-substitution
    echo "BRANCH_TAG=$BRANCH_TAG" > $NANOSAUR_DATA/.env
    echo "NANOSAUR_COVER_TYPE=$NANOSAUR_COVER_TYPE" >> $NANOSAUR_DATA/.env
    echo "RMW_IMPLEMENTATION=$RMW_IMPLEMENTATION" >> $NANOSAUR_DATA/.env
    if [[ $PLATFORM = "robot" ]] ; then
        echo "DOCKER_NETWORK=$DOCKER_NETWORK" >> $NANOSAUR_DATA/.env
        echo "DOCKER_TAG=$DOCKER_TAG" >> $NANOSAUR_DATA/.env
        echo "PERCEPTION_DOCKER_TAG=$PERCEPTION_DOCKER_TAG" >> $NANOSAUR_DATA/.env
    elif [[ $PLATFORM = "desktop" ]] ; then
        echo "IGNITION_VERSION=$IGNITION_VERSION" >> $NANOSAUR_DATA/.env
        echo "SIMULATOR_INSTALLED=$SIMULATOR_INSTALLED" >> $NANOSAUR_DATA/.env
        echo "NANOSAUR_SIMULATION=$NANOSAUR_SIMULATION" >> $NANOSAUR_DATA/.env
    fi
    if [ ! -z ${ROS_DOMAIN_ID+x} ] ; then
        echo "ROS_DOMAIN_ID=$ROS_DOMAIN_ID" >> $NANOSAUR_DATA/.env
    fi
}

############################ BRANCH/DISTRO functions ###########################################

ColorMenu(){
    if [ "$1" == "$3" ] ; then
	    echo -ne "${bold}${green}$2)${reset} ${bold}$3${reset}"
    else
        echo -ne "${blue}$2${reset}) $3"
    fi
}

cover()
{
    # https://devdojo.com/bobbyiliev/how-to-create-an-interactive-menu-in-bash
    local arguments=$1
echo -ne "Cover type: [${bold}$NANOSAUR_COVER_TYPE${reset}]
$(ColorMenu $NANOSAUR_COVER_TYPE '1' 'fisheye')
$(ColorMenu $NANOSAUR_COVER_TYPE '2' 'pi') 
$(ColorMenu $NANOSAUR_COVER_TYPE '3' 'realsense') 
$(ColorMenu $NANOSAUR_COVER_TYPE '4' 'zed')
Chose an option (Q for Exit): "
    read a
    case $a in
        1) NANOSAUR_COVER_TYPE="fisheye" ;;
        2) NANOSAUR_COVER_TYPE="pi" ;;
        3) NANOSAUR_COVER_TYPE="realsense" ;;
        4) NANOSAUR_COVER_TYPE="zed" ;;
    q|Q) exit 0 ;;
    *) echo -e ${red}"Wrong option."${reset};;
    esac

    echo "${bold}${green}Cover:${reset} $NANOSAUR_COVER_TYPE" >&2
    # Align docker with camera name
    if [ "$NANOSAUR_COVER_TYPE" == 'pi' ] || [ "$NANOSAUR_COVER_TYPE" == 'fisheye' ]; then
        PERCEPTION_DOCKER_TAG=$DOCKER_TAG
    else
        PERCEPTION_DOCKER_TAG=$DOCKER_TAG-$NANOSAUR_COVER_TYPE
    fi
    # Store new cover type
    config_save
}

set_network()
{
    local arguments=$1
echo -ne "Network: [${bold}$DOCKER_NETWORK${reset}]
$(ColorMenu $DOCKER_NETWORK '1' 'host')
$(ColorMenu $DOCKER_NETWORK '2' 'bridge')
Chose an option (Q for Exit): "
    read a
    case $a in
        1) DOCKER_NETWORK="host" ;;
        2) DOCKER_NETWORK="bridge" ;;
    q|Q) exit 0 ;;
    *) echo -e ${red}"Wrong option."${reset};;
    esac
    echo "${bold}${green}Network:${reset} $DOCKER_NETWORK" >&2
    # Saving parameters
    config_save
}

set_dds()
{
    local arguments=$1
echo -ne "DDS: [${bold}$RMW_IMPLEMENTATION${reset}]
$(ColorMenu $RMW_IMPLEMENTATION '1' 'rmw_cyclonedds_cpp')
$(ColorMenu $RMW_IMPLEMENTATION '2' 'rmw_fastrtps_cpp')
Chose an option (Q for Exit): "
    read a
    case $a in
        1) RMW_IMPLEMENTATION="rmw_cyclonedds_cpp" ;;
        2) RMW_IMPLEMENTATION="rmw_fastrtps_cpp" ;;
    q|Q) exit 0 ;;
    *) echo -e ${red}"Wrong option."${reset};;
    esac
    echo "${bold}${green}DDS implementation:${reset} $RMW_IMPLEMENTATION" >&2
    # Saving parameters
    config_save
}

set_branch()
{
    local arguments=$1
    if [ ! -z "$arguments" ] ; then
        BRANCH_TAG=$arguments
    else
        echo -ne "Branch: [${bold}$BRANCH_TAG${reset}]
$(ColorMenu $BRANCH_TAG '1' $NANOSAUR_VERSION)
$(ColorMenu $BRANCH_TAG '2' 'foxy')
$(ColorMenu $BRANCH_TAG '3' 'galactic')
Chose an option (Q for Exit): "
        read a
        case $a in
            1) BRANCH_TAG="$NANOSAUR_VERSION" ;;
            2) BRANCH_TAG="foxy" ;;
            3) BRANCH_TAG="galactic" ;;
        q|Q) exit 0 ;;
        *) echo -e ${red}"Wrong option."${reset};;
        esac
    fi
    # Save configuration
    config_save
    echo "${bold}Branch:${reset} ${green}$BRANCH_TAG${reset}" >&2
}

set_distro()
{
    local arguments=$1
    # Update branch
    if [ ! -z "$arguments" ] ; then
        DOCKER_TAG=$arguments
        exit 0
    else
        echo -ne "Branch: [${bold}$DOCKER_TAG${reset}]
$(ColorMenu $DOCKER_TAG '1' 'latest')
$(ColorMenu $DOCKER_TAG '2' 'foxy')
$(ColorMenu $DOCKER_TAG '3' 'galactic')
Chose an option (Q for Exit): "
        read a
        case $a in
            1) DOCKER_TAG="latest" ;;
            2) DOCKER_TAG="foxy" ;;
            3) DOCKER_TAG="galactic" ;;
        q|Q) exit 0 ;;
        *) echo -e ${red}"Wrong option."${reset};;
        esac
    fi

    if [ $DOCKER_TAG = "latest" ] ; then
        BRANCH_TAG=$NANOSAUR_VERSION
    else
        BRANCH_TAG=$DOCKER_TAG
    fi
    # Save configuration
    config_save
    echo "${bold}Distro:${reset} ${green}$DOCKER_TAG${reset}" >&2
}

set_domain()
{
    local arguments=$1
    # https://docs.ros.org/en/ros2_documentation/foxy/Concepts/About-Domain-ID.html
    if [ ! -z "$arguments" ] ; then
        # Check if is a number
        local re='^[0-9]+$'
        if ! [[ $arguments =~ $re ]] ; then
            echo "${red}error: Not a number${reset}" >&2
            exit 1
        fi
        # Set ROS_DOMAIN_ID
        ROS_DOMAIN_ID=$arguments
        config_save
    fi
    # Read ROS_DOMAIN_ID variable
    local domain=0
    if [ -z ${ROS_DOMAIN_ID+x} ] ; then
        echo "${yellow}Unset variable${reset}"
    else
        domain=$ROS_DOMAIN_ID
    fi
    echo "ROS_DOMAIN_ID=$domain"
}

set_simulation_version()
{
    local arguments=$1
    # https://docs.ros.org/en/ros2_documentation/foxy/Concepts/About-Domain-ID.html
    if [ ! -z "$arguments" ] ; then
        # Set ROS_DOMAIN_ID
        IGNITION_VERSION=$arguments
        config_save
    fi
    # Read ROS_DOMAIN_ID variable
    echo "IGNITION_VERSION=$IGNITION_VERSION"
}

############################ SIMULATION functions #############################################

simulation_usage()
{
    if [ "$1" != "" ]; then
        echo "${red}$1${reset}" >&2
    fi

    echo "Control the nanosaur simulation" >&2
    echo "nanosaur simulation [options]" >&2
    echo "${bold}options:${reset}" >&2
    echo "   run                | Run default simulation" >&2
    echo "   set                | Set default simulation" >&2
    echo "   -h|--help          | This help" >&2
}

set_simulation()
{
    local arguments=$1
    # Update branch
    if [ ! -z "$arguments" ] ; then
        NANOSAUR_SIMULATION=$arguments
        exit 0
    else
        echo -ne "Branch: [${bold}$NANOSAUR_SIMULATION${reset}]
$(ColorMenu $NANOSAUR_SIMULATION '1' 'gazebo')
$(ColorMenu $NANOSAUR_SIMULATION '2' 'ignition')
$(ColorMenu $NANOSAUR_SIMULATION '3' 'isaac_sim')
Chose an option (Q for Exit): "
        read a
        case $a in
            1) NANOSAUR_SIMULATION="gazebo" ;;
            2) NANOSAUR_SIMULATION="ignition" ;;
            3) NANOSAUR_SIMULATION="isaac_sim" ;;
        q|Q) exit 0 ;;
        *) echo -e ${red}"Wrong option."${reset};;
        esac
    fi

    # Save configuration
    config_save
    echo "${bold}Simulation:${reset} ${green}$NANOSAUR_SIMULATION${reset}" >&2
}

simulation()
{
    local run_simulation=false
    while [ -n "$1" ]; do
        case "$1" in
            -h|--help) # Load help
                simulation_usage
                exit 0
                ;;
            run) # Run simulation
                run_simulation=true
                ;;
            set) # Set simulator
                set_simulation
                exit 0
                ;;
            *)
                simulation_usage "[ERROR] Unknown option: $1" >&2
                exit 1
                ;;
        esac
            shift 1
    done

    if $run_simulation ; then
        if [[ $NANOSAUR_SIMULATION = "gazebo" ]] ; then
            # Launch nanosaur ignition
            ros2 launch nanosaur_gazebo gazebo.launch.py
        elif [[ $NANOSAUR_SIMULATION = "ignition" ]] ; then
            # Launch nanosaur ignition
            ros2 launch nanosaur_ignition ignition.launch.py
        elif [[ $NANOSAUR_SIMULATION = "isaac_sim" ]] ; then
            # Run NVIDIA Isaac SIM in a new terminal
            local isaac_sim="nanosaur_isaac_sim/scripts/launcher.sh"
            local DIRNAME=$NANOSAUR_CORE_WS_PATH/src/nanosaur_simulations
            gnome-terminal --working-directory=$DIRNAME -- sh -c "bash -c \"echo 'RUN Isaac SIM'; exec bash $isaac_sim\""
            # Run nanosaur isaac_sim ros2 launcer
            ros2 launch nanosaur_isaac_sim isaac_sim.launch.py
        else
            echo "${red}Unknown${reset} ${bold}$NANOSAUR_SIMULATION${reset} ${red}simulator${reset}" >&2
        fi
    else
        simulation_usage
    fi
}

############################ BUILD functions ##################################################

build_usage()
{
    if [ "$1" != "" ]; then
        echo "${red}$1${reset}" >&2
    fi

    echo "This script build all workspaces." >&2
    echo "nanosaur build [options]" >&2
    echo "${bold}options:${reset}" >&2
    echo "   clean                | Clean build folder" >&2
    echo "   -h|--help            | This help" >&2
    echo "   -v|--verbose         | Verbose build" >&2
}

build_clean()
{
    # Run update function
    local THIS="$(pwd)"

    # Go to catkin path
    cd $NANOSAUR_CORE_WS_PATH

    echo "Clean build folders in ${bold}$ROS_CORE_WS_NAME${reset}" >&2

    if [ -d build ] ; then
        rm -R build
    fi
    if [ -d install ] ; then
        rm -R install
    fi
    if [ -d log ] ; then
        rm -R log
    fi
    # Return to main path
    cd $THIS
}

build()
{
    local verbose_build=false
    while [ -n "$1" ]; do
        case "$1" in
            -h|--help) # Load help
                build_usage
                exit 0
                ;;
            -v|--verbose)
                verbose_build=true
                ;;
            clean)
                build_clean
                exit 0
                ;;
            *)
                build_usage "[ERROR] Unknown option: $1" >&2
                exit 1
                ;;
        esac
            shift 1
    done

    # Run update function
    local THIS="$(pwd)"
    # Load ROS2 workspace
    source $ROS2_PATH
    # Go to catkin path
    cd $NANOSAUR_CORE_WS_PATH
    # Load igntion variable
    if $SIMULATOR_INSTALLED ; then
        export IGNITION_VERSION=$IGNITION_VERSION
        echo " ${bold}*${reset} ${green}Ignition build version:${reset} $IGNITION_VERSION" >&2
    fi
    # Catkin make all workspace
    echo " ${bold}*${reset} ${green}colcon build${reset} $(basename $NANOSAUR_CORE_WS_PATH)" >&2
    if $verbose_build ; then
        colcon build --symlink-install --merge-install --event-handlers console_direct+
    else
        colcon build --symlink-install --merge-install
    fi
    # Return to main path
    cd $THIS
}

############################ UPDATE functions #################################################

update_usage()
{
    if [ "$1" != "" ]; then
        echo "${red}$1${reset}" >&2
    fi

    echo "This script update your robot or your desktop." >&2
    echo "nanosaur update [options]" >&2
    echo "${bold}options:${reset}" >&2
    if [[ $PLATFORM = "desktop" ]] ; then
        echo "   rosinstall           | Download all rosinstall dependecies" >&2
    else
        echo "   --clean              | Clean docker after update" >&2
    fi
    echo "   -h|--help            | This help" >&2
}

update_nanosaur_tools()
{
    local branch=$BRANCH_TAG
    # Update nanosaur script and bash completition script
    if [ ! -d $NANOSAUR_CORE_WS_PATH ] ; then
        echo " - ${bold}${green}Pull nanosaur command${reset} and copy in $NANOSAUR_DATA" >&2
        curl https://raw.githubusercontent.com/rnanosaur/nanosaur/$branch/nanosaur/scripts/nanosaur -o $NANOSAUR_DATA/nanosaur
        chmod +x $NANOSAUR_DATA/nanosaur

        echo " - ${bold}${green}Pull nanosaur bash completition${reset} in $NANOSAUR_DATA" >&2
        curl -sSL https://raw.githubusercontent.com/rnanosaur/nanosaur/$branch/nanosaur/scripts/completition.bash  -o $NANOSAUR_DATA/completition.bash
    fi
}

update_developer()
{
    # Update core rosinstall packages
    if [ -d $NANOSAUR_CORE_WS_PATH/src ] ; then
        # Import workspace
        if [ -d $NANOSAUR_CORE_WS_PATH/src/nanosaur/nanosaur/rosinstall ] ; then
            echo " - ${green}Check status all workspaces${reset} Branch: ${bold}$BRANCH_TAG${reset}" >&2
            local rosinstall_name="desktop.rosinstall"
            if [[ $PLATFORM = "robot" ]] ; then
                rosinstall_name="robot.rosinstall"
            fi
            if $SIMULATOR_INSTALLED ; then
                rosinstall_name="simulations.rosinstall"
            fi
            vcs import $NANOSAUR_CORE_WS_PATH/src < $NANOSAUR_CORE_WS_PATH/src/nanosaur/nanosaur/rosinstall/$rosinstall_name
            vcs pull $NANOSAUR_CORE_WS_PATH/src
        else
            echo " - ${bold}${green}Download and update Nanosaur rosinstall${reset}" >&2
            curl $(get_nanosaur_rosinstall) -o $NANOSAUR_DATA/nanosaur.rosinstall
            vcs import $NANOSAUR_CORE_WS_PATH/src < $NANOSAUR_DATA/nanosaur.rosinstall
            rm $NANOSAUR_DATA/nanosaur.rosinstall
        fi
        # Status update
        if [ $? -ne 0 ] ; then
            # Try to make again
            echo "${red}Please check workspace status ${reset}" >&2
            # sudo_me false
            exit 1
        fi
    fi

    # Update package perception on robot
    if [[ $PLATFORM = "robot" ]] ; then
        if [ -d $NANOSAUR_PERCEPTION_WS_PATH/src ] ; then
            # Import workspace
            if [ -d $NANOSAUR_PERCEPTION_WS_PATH/src/nanosaur_perception/nanosaur_perception/rosinstall ] ; then
                echo " - ${green}Check status all perception workspaces${reset} Branch: ${bold}$BRANCH_TAG${reset}" >&2
                local path_perception=$NANOSAUR_PERCEPTION_WS_PATH/src/nanosaur_perception/nanosaur_perception/rosinstall/perception.rosinstall
                vcs import $NANOSAUR_PERCEPTION_WS_PATH/src < $path_perception
                vcs pull $NANOSAUR_PERCEPTION_WS_PATH/src
            else
                echo " - ${bold}${green}Download and update perception rosinstall${reset}" >&2
                curl $(get_perception_rosinstall) -o $NANOSAUR_DATA/perception.rosinstall
                vcs import $NANOSAUR_PERCEPTION_WS_PATH/src < $NANOSAUR_DATA/perception.rosinstall
                rm $NANOSAUR_DATA/perception.rosinstall
            fi
            # Status update
            if [ $? -ne 0 ] ; then
                # Try to make again
                echo "${red}Please check workspace status ${reset}" >&2
                # sudo_me false
                exit 1
            fi
        fi
    fi
}

update_on_robot()
{
    local CLEAN=$1
    # Switch off nanosaur
    docker-compose -f $NANOSAUR_DATA/docker-compose.yml down
    # Update docker-compose
    if [ -d $NANOSAUR_CORE_WS_PATH ] ; then
        if [ ! -L $NANOSAUR_DATA/docker-compose.yml ] ; then
            echo " - ${bold}${green}Link Nanosaur docker-compose${reset}" >&2
            ln -s  $NANOSAUR_CORE_WS_PATH/src/nanosaur/docker-compose.yml $NANOSAUR_DATA/docker-compose.yml
        fi
    else
        # Download latest version nanosaur docker-compose
        echo " - ${bold}${green}Download Nanosaur docker-compose${reset}" >&2
        # Download the docker-compose image and run
        local branch=$BRANCH_TAG
        curl https://raw.githubusercontent.com/rnanosaur/nanosaur/$branch/docker-compose.yml -o $NANOSAUR_DATA/docker-compose.yml
    fi
    echo " - ${bold}${green}Pull and restart all containers DISTRO: $DOCKER_TAG${reset}" >&2
    # Pull all new images
    docker-compose -f $NANOSAUR_DATA/docker-compose.yml pull
    # Clean images after pull
    if $CLEAN ; then
        docker image prune -f
    fi
}

update()
{
    local rosinstall=false
    local CLEAN=false
	# Decode all information from startup
    while [ -n "$1" ]; do
        case "$1" in
            -h|--help) # Load help
                update_usage
                exit 0
                ;;
            rosinstall)
                if [[ $PLATFORM = "desktop" ]] ; then
                    rosinstall=true
                fi
                ;;
            --clean)
                CLEAN=true
                ;;
            *)
                update_usage "[ERROR] Unknown option: $1" >&2
                exit 1
                ;;
        esac
            shift 1
    done

    if $rosinstall ; then
        # Request sudo password
        sudo -v
        sudo_me true
    fi

    # Update all nanosaur tools
    update_nanosaur_tools
    # Update developer packages
    update_developer

    if $rosinstall ; then
        # Load ROS2 workspace
        source $ROS2_PATH
        echo " - ${bold}${green}Update rosdep${reset}" >&2
        # Update rosdep
        rosdep update
        # Install all dependencies
        # http://wiki.ros.org/rosdep
        echo " ${bold}*${reset} Install all ${bold}dependencies${reset}" >&2
        rosdep install --from-paths $NANOSAUR_CORE_WS_PATH/src --ignore-src -r -y

        # Disable sudo me
        sudo_me false
    fi

    # Install Docker and other scripts
    if [[ $PLATFORM = "robot" ]] ; then
        update_on_robot $CLEAN
    fi
}

############################ INSTALL functions ################################################

installer_usage()
{
    if [ "$1" != "" ]; then
        echo "${red}$1${reset}" >&2
    fi

    echo "Use this option to install nansaur on your desktop or your NVIDIA Jetson" >&2
    echo "nanosaur install [options]" >&2
    echo "${bold}options:${reset}" >&2
    echo "   developer            | Install nanosaur in developer mode (install in ${bold}$NANOSAUR_CORE_WS_PATH${reset} workspace)" >&2
    echo "   simulation           | Install simulation packages (install in ${bold}$NANOSAUR_CORE_WS_PATH${reset} workspace)"
    echo "   -y                   | Run this script silent" >&2
    echo "   --force              | Force to install nanosaur anyway" >&2
    echo "   -h|--help            | This help" >&2
}

install_nanosaur_tools()
{
    # Check if exist the nanosaur workspace
    if [ -d $NANOSAUR_CORE_WS_PATH ] ; then
        if [ -f $NANOSAUR_DATA/nanosaur ] ; then
            rm $NANOSAUR_DATA/nanosaur 
        fi
        if [ ! -L $NANOSAUR_DATA/nanosaur ] ; then
            echo " - Link nanosaur command in ${bold}${green}$NANOSAUR_DATA${reset}"
            ln -s $NANOSAUR_CORE_WS_PATH/src/nanosaur/nanosaur/scripts/nanosaur $NANOSAUR_DATA/nanosaur
        fi
        if [ ! -L $NANOSAUR_DATA/completition.bash ] ; then
            echo " - Link nanosaur completition in ${bold}${green}$NANOSAUR_DATA${reset}"
            ln -s $NANOSAUR_CORE_WS_PATH/src/nanosaur/nanosaur/scripts/completition.bash $NANOSAUR_DATA/completition.bash
        fi
    else
        echo " - ${bold}${green}Move nanosaur command${reset} in $NANOSAUR_DATA"
        mv $HOME/nanosaur $NANOSAUR_DATA/nanosaur
        chmod +x $NANOSAUR_DATA/nanosaur

        echo " - ${bold}${green}Copy nanosaur bash completition${reset} in $NANOSAUR_DATA"
        curl -sSL https://raw.githubusercontent.com/rnanosaur/nanosaur/master/nanosaur/scripts/completition.bash  -o $NANOSAUR_DATA/completition.bash
    fi

    # link nanosaur script to nanosaur path
    if [ ! -L /usr/local/bin/nanosaur ] ; then
        echo " - Link nanosaur command in ${bold}${green}/usr/local/bin${reset}"
        sudo ln -s $NANOSAUR_DATA/nanosaur /usr/local/bin/nanosaur
    fi

    # link nanosaur bash completition
    if [ ! -L /etc/bash_completion.d/nanosaur ] ; then
        echo " - Link nanosaur bash completition in ${bold}${green}/etc/bash_completion.d/${reset}"
        sudo ln -s $NANOSAUR_DATA/completition.bash /etc/bash_completion.d/nanosaur
    fi
}

install_developer()
{
    if ! command -v vcs &> /dev/null ; then
        echo " - Install vcs tool"
        sudo apt update && sudo apt install curl gnupg2 lsb-release -y
        sudo curl -sSL https://raw.githubusercontent.com/ros/rosdistro/master/ros.key  -o /usr/share/keyrings/ros-archive-keyring.gpg
        echo "deb [arch=$(dpkg --print-architecture) signed-by=/usr/share/keyrings/ros-archive-keyring.gpg] http://packages.ros.org/ros2/ubuntu $(lsb_release -cs) main" | sudo tee /etc/apt/sources.list.d/ros2.list > /dev/null
        sudo apt update && sudo apt-get install python3-vcstool -y
    fi

    # make folder for nanosaur core workspace
    if [ ! -d $NANOSAUR_CORE_WS_PATH ] ; then
        echo " - ${bold}${green}Make nanosaur workspace folder in $NANOSAUR_CORE_WS_PATH${reset}"
        mkdir -p $NANOSAUR_CORE_WS_PATH/src 
    fi

    # make folder for nanosaur perception workspace
    if [[ $PLATFORM = "robot" ]] ; then
        if [ ! -d $NANOSAUR_PERCEPTION_WS_PATH ] ; then
            echo " - ${bold}${green}Make nanosaur_perception workspace folder in $NANOSAUR_PERCEPTION_WS_PATH${reset}"
            mkdir -p $NANOSAUR_PERCEPTION_WS_PATH/src 
        fi
    fi
}

install_on_desktop()
{
    if ! command -v rosdep &> /dev/null ; then
        echo " - ${bold}${green}Install rosdep${reset}"
        sudo apt-get install -y python3-rosdep
    fi
    echo " - ${bold}${green}Install/Update python-dotenv${reset}"
    pip3 install -U python-dotenv --no-warn-script-location

    if ! command -v docker &> /dev/null ; then
        echo " - ${bold}${green}Install docker & docker compose 2${reset}"
        # Install certificates
        sudo apt-get install -y ca-certificates curl gnupg lsb-release
        # Add Docker’s official GPG key
        sudo mkdir -p /etc/apt/keyrings
        curl -fsSL https://download.docker.com/linux/ubuntu/gpg | sudo gpg --dearmor -o /etc/apt/keyrings/docker.gpg
        # Set up the repository
        echo "deb [arch=$(dpkg --print-architecture) signed-by=/etc/apt/keyrings/docker.gpg] https://download.docker.com/linux/ubuntu $(lsb_release -cs) stable" | sudo tee /etc/apt/sources.list.d/docker.list > /dev/null
        # Install Docker Engine
        sudo apt-get update
        sudo apt-get install -y docker-ce docker-ce-cli containerd.io docker-compose-plugin
    fi

    if ! getent group docker | grep -q "\b$USER\b" ; then
        echo " - Add docker permissions to ${bold}${green}user=$USER${reset}"
        sudo usermod -aG docker $USER
    fi
}

install_on_robot()
{
    if ! command -v pip3 &> /dev/null ; then
        echo " - ${bold}${green}Install pip/pip3${reset}"
        sudo apt-get install -y python3-pip
    fi

    # Check if is installed nano
    if ! command -v nano &> /dev/null ; then
        echo " - ${bold}${green}Install nano${reset}"
        sudo apt-get install -y nano
    fi

    # Check if is installed jtop
    if ! command -v jtop &> /dev/null ; then
        echo " - ${bold}${green}Install/Update jetson-stats${reset}"
        sudo -H pip3 install -U jetson-stats
    fi

    # Check if is installed ros2_system_manager
    if [ ! -f /usr/lib/ros2_system_manager/system_manager_server ] ; then
        echo " - ${bold}${green}Install/Update ros2_system_manager${reset}"
        sudo -H pip3 install -U ros2-system-manager
    fi

    if ! getent group docker | grep -q "\b$USER\b" ; then
        echo " - Add docker permissions to ${bold}${green}user=$USER${reset}"
        sudo usermod -aG docker $USER
    fi

    # Check if is installed docker-compose
    if ! command -v docker-compose &> /dev/null ; then
        echo " - ${bold}${green}Install docker-compose${reset}"
        sudo apt-get install -y libffi-dev python-openssl libssl-dev
        sudo -H pip3 install -U pip
        sudo pip3 install -U docker-compose
    fi

    local branch=$BRANCH_TAG

    if [ -d $NANOSAUR_CORE_WS_PATH ] ; then
        if [ ! -L $NANOSAUR_DATA/docker-compose.yml ] ; then
            echo " - ${bold}${green}Link Nanosaur docker-compose${reset}" >&2
            ln -s  $NANOSAUR_CORE_WS_PATH/src/nanosaur/docker-compose.yml $NANOSAUR_DATA/docker-compose.yml
        fi
    else
        # Download latest version nanosaur docker-compose
        echo " - ${bold}${green}Download Nanosaur docker-compose${reset}" >&2
        # Download the docker-compose image and run
        curl https://raw.githubusercontent.com/rnanosaur/nanosaur/$branch/docker-compose.yml -o $NANOSAUR_DATA/docker-compose.yml
    fi

    # Make sure the nvidia docker runtime will be used for builds
    local DEFAULT_RUNTIME=$(docker info | grep "Default Runtime: nvidia" ; true)
    if [[ -z "$DEFAULT_RUNTIME" ]]; then
        echo "${green} - Download docker daemon.json${reset}"
        curl https://raw.githubusercontent.com/rnanosaur/nanosaur/$branch/nanosaur/scripts/daemon.json -o $NANOSAUR_DATA/daemon.json
        echo "${green} - Set runtime nvidia on /etc/docker/daemon.json${reset}"
        sudo mv /etc/docker/daemon.json /etc/docker/daemon.json.bkp
        sudo cp $NANOSAUR_DATA/daemon.json /etc/docker/daemon.json
        echo "${green} - Restart docker service${reset}"
        sudo systemctl restart docker.service
    fi

    # Create nanosaur docker
    echo " - ${bold}${green}Start nanosaur docker-compose${reset}"
    # TODO: make .env file before start
    sudo docker-compose -f $NANOSAUR_DATA/docker-compose.yml up -d
}

installer()
{
    local developer=false
    local simulation=false
    local SILENT=false
    local FORCE=false
	# Decode all information from startup
    while [ -n "$1" ]; do
        case "$1" in
            -h|--help) # Load help
                installer_usage
                exit 0
                ;;
            --force)
                FORCE=true
                ;;
            -y)
                SILENT=true
                ;;
            developer)
                developer=true
                ;;
            simulation)
                simulation=true
                ;;
            *)
                installer_usage "[ERROR] Unknown option: $1" >&2
                exit 1
                ;;
        esac
            shift 1
    done

    # Check if is installed on the same Jetpack
    if [[ $PLATFORM = "robot" ]] ; then
        # Read version
        local JETSON_L4T_STRING=$(dpkg-query --showformat='${Version}' --show nvidia-l4t-core)
        # extract version
        local JETSON_L4T_ARRAY=$(echo $JETSON_L4T_STRING | cut -f 1 -d '-')
        # Load release and revision
        local JETSON_L4T_RELEASE=$(echo $JETSON_L4T_ARRAY | cut -f1,2 -d '.')
        local JETSON_L4T_REVISION=${JETSON_L4T_ARRAY#"$JETSON_L4T_RELEASE."}

        if [[ $JETSON_L4T_RELEASE != $NANOSAUR_L4T ]] ; then
            echo "${bold}${red}You cannot install nanosaur on this Jetpack with L4T $JETSON_L4T_RELEASE need L4T $NANOSAUR_L4T ${reset}"
            if ! $FORCE ; then
                exit 0
            fi
        fi
    fi

    local type_developer=""
    # Recap installatation
    echo "------ Configuration ------"
    echo " - ${bold}NVIDIA L4T:${reset} ${green}$JETSON_L4T_RELEASE.$JETSON_L4T_REVISION${reset}"
    echo " - ${bold}User:${reset} ${green}$USER${reset} - ${bold}Hostname:${reset} ${green}$HOSTNAME${reset}"
    echo " - ${bold}Install on:${reset} ${green}$PLATFORM${reset}"
    echo " - ${bold}Developer:${reset} ${green}$developer${reset}"
    if [[ $PLATFORM = "desktop" ]] ; then
        echo " - ${bold}Simulation:${reset} ${blue}$simulation${reset}"
    fi
    config_info
    echo "---------------------------"

    while ! $SILENT; do
        read -p "Do you wish to install nanosaur on this platform? [Y/n] " yn
            case $yn in
                [Yy]* ) # Break and install jetson_stats 
                        break;;
                [Nn]* ) exit;;
            * ) echo "Please answer yes or no.";;
        esac
    done

    # curl https://raw.githubusercontent.com/rnanosaur/nanosaur/master/nanosaur/scripts/nanosaur -o $HOME/nanosaur && \
    # chmod +x $HOME/nanosaur && nanosaur install

    # Request sudo password
    sudo -v

    if [[ $PLATFORM = "robot" ]] ; then
        echo "${bold}${green}Install nanosaur on Jetpack L4T $JETSON_L4T_RELEASE.$JETSON_L4T_REVISION ${reset}"
    fi

    if [ ! -d $NANOSAUR_DATA ] ; then
        echo " - ${bold}${green}Make nanosaur folder in $NANOSAUR_DATA${reset}"
        # Build nanosaur folder structure
        # - /opt/nanosaur
        # -      /param [ ros2 parameter folder ]
        sudo mkdir -p $NANOSAUR_DATA
        sudo chown $USER:$USER $NANOSAUR_DATA
        mkdir -p "$NANOSAUR_DATA/param"
    fi

    # Enable simulation
    if $simulation ; then
        SIMULATOR_INSTALLED=true
        # Saving parameters
        config_save
    fi
    # Install developer tools
    if $developer ; then
        # Make folders
        install_developer
    fi
    # Install nanosaur tools
    install_nanosaur_tools
    # Install Docker and other scripts
    if [[ $PLATFORM = "robot" ]] ; then
        # install all services
        install_on_robot
        # Populate folders if exists
        update_developer
    else
        # Install software dependencies
        install_on_desktop
        # Run ronsintall
        update rosinstall
        # Run builder
        build
    fi

    if [ -f /var/run/reboot-required ] ; then
        # After install require reboot
        echo "${red}*** System Restart Required ***${reset}"
    fi
}

############################ INFO functions ####################################################

info()
{
    local verbose=false
	# Decode all information from startup
    while [ -n "$1" ]; do
        case "$1" in
            -h|--help) # Load help
                info_usage
                exit 0
                ;;
            -v|--verbose)
                verbose=true
                ;;
            *)
                install_usage "[ERROR] Unknown option: $1" >&2
                exit 1
                ;;
        esac
            shift 1
    done

    # Status configurations
    local arch="$(uname -m)"
    echo "${bold}Configuration:${reset}" >&2
    echo " - ${bold}Architecture:${reset} ${green}$arch${reset} - ${bold}type:${reset} ${green}$PLATFORM${reset}" >&2
    echo " - ${bold}nanosaur folder:${reset} ${green}$NANOSAUR_DATA${reset}" >&2
    config_info
    if $verbose ; then
        # Status workspaces
        if [ -d $NANOSAUR_CORE_WS_PATH/src ] ; then
            echo "-------------------------------------------------------------------------" >&2
            echo "${bold}Status $ROS_CORE_WS_NAME:${reset}"
            vcs status "$NANOSAUR_CORE_WS_PATH/src"
        fi
        if [ -d $NANOSAUR_PERCEPTION_WS_PATH/src ] ; then
            echo "-------------------------------------------------------------------------" >&2
            echo "${bold}Status $ROS_PERC_WS_NAME:${reset}"
            vcs status "$NANOSAUR_PERCEPTION_WS_PATH/src"
        fi
    fi
    # Status Docker
    if [[ $PLATFORM = "robot" ]] ; then
        echo "-------------------------------------------------------------------------" >&2
        echo "${bold}Docker status:${reset}"
        docker-compose -f $NANOSAUR_DATA/docker-compose.yml ps -a
    fi
    echo "-------------------------------------------------------------------------" >&2
}

############################ MAIN function #####################################################

usage()
{
    if [ "$1" != "" ]; then
        echo "${red}$1${reset}" >&2
    fi

    local name=$(basename ${0})
    echo "$name is your manager to control, update your robot or your desktop to work with nanosaur." >&2
    echo "${bold}Commands:${reset}" >&2
    echo "  $name help                     This help" >&2
    echo "  $name info                     Status" >&2
    echo "  $name cover                    Set cover type" >&2
    echo "  $name config                   Open/create robot.yml in ${bold}$NANOSAUR_DATA/param${reset} (use nano)" >&2
    echo "  $name dds                      Set DDS communication RMW_IMPLEMENTATION" >&2
    echo "  $name domain                   Set ROS_DOMAIN_ID" >&2 # https://docs.ros.org/en/ros2_documentation/foxy/Concepts/About-Domain-ID.html
    echo "  $name install                  Install nanosaur environment." >&2
    echo "  $name update                   Update docker and scripts" >&2
    if [[ $PLATFORM = "desktop" ]] ; then
        echo "  $name build                    Build developer boards" >&2
        echo "  $name branch                   Set branch nanosaur projects" >&2
        echo "  $name perception               Run docker with Isaac ROS perception" >&2
        echo "  $name teleop                   Run the nanosaur keyboard controller" >&2
    else
        echo "  $name distro                   Set nanosaur distribution" >&2
        echo "  $name network [host/bridge]    Select network configuration" >&2
        echo "  $name wakeup/down              start/stop nanosaur systems (eq. nanosaur up -d)" >&2
        echo "  $name [start/restart/stop/up/logs/top/rm/exec]  Control nanosaur docker" >&2
        echo "  $name run [CNT:core][CMD:bash] Run a new nanosaur container for development" >&2
        echo "  $name clean                    clean all dangling images" >&2
    fi
    if $SIMULATOR_INSTALLED ; then
        echo "${bold}Simulation info:${reset}"
        echo "  $name simulation               Start the nanosaur simulator" >&2
        echo "  $name ignition                 Set simulation Ignition version. Default: $IGNITION_VERSION" >&2
    fi
    echo ""
    echo "$name managager ${bold}v$NANOSAUR_VERSION${reset} | Configuration folder is in ${bold}$NANOSAUR_DATA${reset}" >&2
}

main()
{
	# Check if run in sudo
    if [[ `id -u` -eq 0 ]] ; then 
        echo "${red}Please don't run as root${reset}" >&2
        exit 1
    fi
    local option=$1
    if [ -z "$option" ] ; then
        usage
        exit 0
    fi
    # Load all arguments except the first one
    local arguments=${@:2}

    # Options
    if [ $option = "help" ] || [ $option = "-h" ]; then
        usage
        exit 0
    elif [ $option = "-v" ] ; then
        echo $NANOSAUR_VERSION
        exit 0
    elif [ $option = "cover" ] ; then
        cover $arguments
        exit 0
    elif [ $option = "info" ] ; then
        info $arguments
        exit 0
    elif [ $option = "config" ] ; then
        nano $ROBOT_FILE_PATH
        exit 0
    elif [ $option = "dds" ] ; then
        set_dds $arguments
        exit 0
    elif [ $option = "domain" ] ; then
        set_domain $arguments
        exit 0
    elif [ $option = "install" ] ; then
        installer $arguments
        exit 0
    elif [ $option = "update" ] ; then
        # Update nanosaur
        update $arguments
        exit 0
    fi

    # Specific options for desktop/robot
    if [[ $PLATFORM = "desktop" ]] ; then
        if [ $option = "build" ] ; then
            build $arguments
            exit 0
        elif [ $option = "branch" ] ; then
            set_branch $arguments
            exit 0
        elif [ $option = "ignition" ] ; then
            set_simulation_version $arguments
            exit 0
        elif [ $option = "perception" ] ; then
            docker compose -f $NANOSAUR_CORE_WS_PATH/src/nanosaur_perception/docker-compose.yml up $arguments
            exit 0
        elif [ $option = "simulation" ] ; then
            simulation $arguments
            exit 0
        elif [ $option = "teleop" ] ; then
            # Load Nanosaur workspace
            source $NANOSAUR_CORE_WS_PATH/install/setup.bash
            eval $nanosaur_keyboard_cmd
            exit 0
        fi
    else
        # Enable all docker options
        if [ $option = "distro" ] ; then
            set_distro $arguments
            exit 0
        elif [ $option = "network" ] ; then
            set_network $arguments
            exit 0
        elif [ $option = "up" ] || [ $option = "start" ] || [ $option = "restart" ] || 
                [ $option = "stop" ] || [ $option = "down" ] || [ $option = "logs" ] || 
                [ $option = "top" ] || [ $option = "rm" ] ||[ $option = "exec" ] ; then
            # Run docker compose
            docker-compose -f $NANOSAUR_DATA/docker-compose.yml $option $arguments
            exit 0
        elif [ $option = "wakeup" ] ; then
            docker-compose -f $NANOSAUR_DATA/docker-compose.yml up -d
            exit 0
        elif [ $option = "clean" ] ; then
            echo "${green}Clean all dangling images${reset}" >&2
            # prune all images
            docker image prune $arguments
            exit 0
        elif [ $option = "run" ] ; then
            # Check if is selected help
            if [ $2 = "--help" ] ; then
                # Get list of all services
                local services=$(docker-compose -f $NANOSAUR_DATA/docker-compose.yml ps --services)
                echo "${yellow}All images available: ${bold}[${services//$'\n'/ }]${reset}" >&2
                exit 0
            fi
            local image_name=$2
            local arguments=${@:3}
            if [ -z $image_name ] ; then
                echo "${yellow}Load default image ${reset}" >&2
                image_name="develop"
            fi
            # Load command option
            local docker_cmd="bash"
            if [ ! -z "$arguments" ] ; then
                docker_cmd=$arguments
            fi
            echo "${green}Run a new nanosaur docker${reset}" >&2
            # Print configuration
            echo "${bold}Configuration:${reset}"
            config_info
            echo "---------------------------------" >&2
            # Check volume
            local WORKSPACE_DOCKER=""
            # Check if exist nanosaur workspace on robot folder
            local domain=""
            if [ ! -z ${ROS_DOMAIN_ID+x} ] ; then
                domain="-e ROS_DOMAIN_ID=$ROS_DOMAIN_ID"
            fi
            # Start docker image
            if [ $image_name = "core" ] ; then
                if [ -d $NANOSAUR_CORE_WS_PATH ] ; then
                    echo " - ${bold}Load volume:${reset} ${yellow}$NANOSAUR_CORE_WS_PATH${reset}" >&2
                    WORKSPACE_DOCKER="-v $NANOSAUR_CORE_WS_PATH/src:/opt/ros_ws/src/"
                fi
                echo "${bold}Running Docker ${blue}nanosaur/nanosaur:$DOCKER_TAG $docker_cmd${reset}"
                # Run docker
                docker run -it --rm --network host \
                            --device /dev/i2c-0 \
                            --device /dev/i2c-1 \
                            --device /dev/input \
                            -v /run/jtop.sock:/run/jtop.sock \
                            -v /run/ros2sm.sock:/run/ros2sm.sock \
                            -e NANOSAUR_COVER_TYPE=$NANOSAUR_COVER_TYPE \
                            -e RMW_IMPLEMENTATION=$RMW_IMPLEMENTATION \
                            $domain \
                            $WORKSPACE_DOCKER \
                            nanosaur/nanosaur:$DOCKER_TAG $docker_cmd
            elif [ $image_name = "perception" ] ; then
                # Load perception workspace
                if [ -d $NANOSAUR_PERCEPTION_WS_PATH ] ; then
                    echo " - ${bold}Load volume:${reset} ${yellow}$NANOSAUR_PERCEPTION_WS_PATH${reset}" >&2
                    WORKSPACE_DOCKER="-v $NANOSAUR_PERCEPTION_WS_PATH/src:/opt/ros_ws/src/"
                fi
                # Load isaac ros workspace
                local ISAAC_WS_DOCKER=""
                if [ -d $ISAAC_ROS_WS_PATH ] ; then
                    echo " - ${bold}Load volume:${reset} ${yellow}$ISAAC_ROS_WS_PATH${reset}" >&2
                    ISAAC_WS_DOCKER="-v $ISAAC_ROS_WS_PATH/src:/opt/isaac_ros_ws/src/"
                fi
                echo "${bold}Running Docker ${blue}nanosaur/perception:$PERCEPTION_DOCKER_TAG $docker_cmd${reset}"
                # Run docker
                docker run --runtime nvidia -it --rm --network host --privileged \
                            -v /tmp/argus_socket:/tmp/argus_socket \
                            -e NANOSAUR_COVER_TYPE=$NANOSAUR_COVER_TYPE \
                            -e RMW_IMPLEMENTATION=$RMW_IMPLEMENTATION \
                            $domain \
                            $WORKSPACE_DOCKER \
                            $ISAAC_WS_DOCKER \
                            nanosaur/perception:$PERCEPTION_DOCKER_TAG $docker_cmd
            else
                echo "${yellow}Load default docker image ${bold}$N1${reset}" >&2
                docker run -it --rm --network host \
                            -e NANOSAUR_COVER_TYPE=$NANOSAUR_COVER_TYPE \
                            -e RMW_IMPLEMENTATION=$RMW_IMPLEMENTATION \
                            $domain \
                            nanosaur/nanosaur:$DOCKER_TAG $docker_cmd
            fi
            # Exit option run
            exit 0
        fi
    fi

    usage "[ERROR] Unknown option: $option" >&2
    exit 1
}


# Detect if is sourced or not
# https://stackoverflow.com/questions/2683279/how-to-detect-if-a-script-is-being-sourced
if [ "${BASH_SOURCE-}" = "$0" ] ; then
    main $@
    exit 0
fi

# Check if is running on Desktop platform
if [ $PLATFORM != "desktop" ] ; then
    echo "${red}You must source this script only from a Desktop machine${reset}" >&2
    return
fi

deactivate () {

    # https://stackoverflow.com/questions/8760505/is-it-possible-to-unsource-in-bash
    # Restore .bashrc
    if [ ! "${1-}" = "nondestructive" ] ; then
        exec bash
        # Load ROS_DOMAIN_ID
        if [ ! -z ${ROS_DOMAIN_ID+x} ] ; then
            unset ROS_DOMAIN_ID
        fi
        # Load NANOSAUR_COVER_TYPE
        if [ ! -z ${NANOSAUR_COVER_TYPE+x} ] ; then
            unset NANOSAUR_COVER_TYPE
        fi
        # Load RMW_IMPLEMENTATION
        if [ ! -z ${RMW_IMPLEMENTATION+x} ] ; then
            unset RMW_IMPLEMENTATION
        fi
    fi

    # This should detect bash and zsh, which have a hash command that must
    # be called to get it to forget past commands.  Without forgetting
    # past commands the $PATH changes we made may not be respected
    if [ -n "${BASH-}" ] || [ -n "${ZSH_VERSION-}" ] ; then
        hash -r 2>/dev/null
    fi

    if ! [ -z "${_OLD_VIRTUAL_PS1+_}" ] ; then
        PS1="$_OLD_VIRTUAL_PS1"
        export PS1
        unset _OLD_VIRTUAL_PS1
    fi

    unset VIRTUAL_ENV
    if [ ! "${1-}" = "nondestructive" ] ; then
    # Self destruct!
        unset -f deactivate
    fi
}

# unset irrelevant variables
deactivate nondestructive

# Check ROS2 workspace and nanosaur workspace
if [ ! -d $(dirname $ROS2_PATH) ] ; then
    echo "${red}ROS2 is not installed in $(dirname $ROS2_PATH)${reset}" >&2
    return
fi
# Load ROS2 workspace
source $ROS2_PATH
# Load ROS_DOMAIN_ID
if [ ! -z ${ROS_DOMAIN_ID+x} ] ; then
    export ROS_DOMAIN_ID=$ROS_DOMAIN_ID
fi
# Load NANOSAUR_COVER_TYPE
if [ ! -z ${NANOSAUR_COVER_TYPE+x} ] ; then
    export NANOSAUR_COVER_TYPE=$NANOSAUR_COVER_TYPE
fi
# Load RMW_IMPLEMENTATION
if [ ! -z ${RMW_IMPLEMENTATION+x} ] ; then
    export RMW_IMPLEMENTATION=$RMW_IMPLEMENTATION
fi

if [ ! -d $NANOSAUR_CORE_WS_PATH/install ] ; then
    echo "${yellow}Nanosaur workspace is missing or not builded check ${bold}$NANOSAUR_CORE_WS_PATH${reset}" >&2
else
    # Load Nanosaur workspace
    source $NANOSAUR_CORE_WS_PATH/install/setup.bash
fi

# Setup PS1
if [ -z "${VIRTUAL_ENV_DISABLE_PROMPT-}" ] ; then
    _OLD_VIRTUAL_PS1="$PS1"
    # Rewrite PS1
    PS1="($ROBOT_NAME) $PS1"
    if [ ! -z ${ROS_DOMAIN_ID+x} ] ; then
        if [ $ROS_DOMAIN_ID -ne 0 ]; then
            PS1="id=$ROS_DOMAIN_ID $PS1"
        fi
    fi
    export PS1
fi

# This should detect bash and zsh, which have a hash command that must
# be called to get it to forget past commands.  Without forgetting
# past commands the $PATH changes we made may not be respected
if [ -n "${BASH-}" ] || [ -n "${ZSH_VERSION-}" ] ; then
    hash -r 2>/dev/null
fi
# EOF