#!/bin/bash
#
# Author: Mike Purvis <mpurvis@clearpathrobotics.com>
#         Copyright (c) 2013, Clearpath Robotics, Inc.
# 
# Redistribution and use in source and binary forms, with or without
# modification, are permitted provided that the following conditions are met:
#    * Redistributions of source code must retain the above copyright
#       notice, this list of conditions and the following disclaimer.
#    * 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.
#    * Neither the name of Clearpath Robotics, Inc. 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 CLEARPATH ROBOTICS, INC. 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.
# 
# Please send comments, questions, or patches to code@clearpathrobotics.com 

function log() {{
  logger -s -p user.$1 ${{@:2}}
}}

log info "{job}: Using workspace setup file {setup}"
source {setup}
JOB_FOLDER=/etc/ros/{rosdistro}/{job}.d 

logdir={logdir}
export ROS_IP=`rosrun robot_upstart getifip {interface}`
export ROS_MASTER_URI={master}
if [ "$ROS_IP" = "" ]; then
  log err "{job}: No IP address on {interface}, cannot roslaunch."
  exit 1
fi

if [[ ! -d $logdir ]]; then
  log warn "{job}: The log directory you specified ($logdir) does not exist. Defaulting to /tmp!"
  logdir="/tmp"
fi

log info "{job}: Launching on interface {interface}, ROS_IP=$ROS_IP, ROS_MASTER_URI=$ROS_MASTER_URI, ROS_LOG_DIR=$logdir"

# If xacro files are present in job folder, generate and expand an amalgamated urdf.
XACRO_FILENAME=$logdir/{job}.xacro
XACRO_ROBOT_NAME=$(echo "{job}" | cut -d- -f1)
rosrun robot_upstart mkxacro $JOB_FOLDER $XACRO_ROBOT_NAME > $XACRO_FILENAME
if [[ "$?" == "0" ]]; then
  URDF_FILENAME=$logdir/{job}.urdf
  rosrun xacro xacro $XACRO_FILENAME -o $URDF_FILENAME
  if [[ "$?" == "0" ]]; then
    log info "{job}: Generated URDF: $URDF_FILENAME"
  else
    log warn "{job}: URDF macro expansion failure. Robot description will not function."
  fi
  export ROBOT_URDF_FILENAME=$URDF_FILENAME
fi

# Assemble amalgamated launchfile.
LAUNCH_FILENAME=$logdir/{job}.launch
rosrun robot_upstart mklaunch $JOB_FOLDER > $LAUNCH_FILENAME
if [[ "$?" != "0" ]]; then
  log err "{job}: Unable to generate amalgamated launchfile."
  exit 1
fi
log info "{job}: Generated launchfile: $LAUNCH_FILENAME"

# Warn and exit if setuidgid is missing from the system.
which setuidgid > /dev/null
if [ "$?" != "0" ]; then
  log err "{job}: Can't launch as unprivileged user without setuidgid. Please install daemontools package."
  exit 1
fi

# Punch it.
export ROS_HOME=$(echo ~{user})/.ros
export ROS_LOG_DIR=$logdir
setuidgid {user} roslaunch $LAUNCH_FILENAME &
PID=$!

log info "{job}: Started roslaunch as background process, PID $PID, ROS_LOG_DIR=$ROS_LOG_DIR"
echo "$PID" > $logdir/{job}.pid

wait "$PID"