#!/usr/bin/env bash
export LD_LIBRARY_PATH=/usr/local/Aria/lib:$LD_LIBRARY_PATH
source /opt/ros/hydro/setup.bash
source /home/lab1_5//ws/ws_linux/devel/setup.bash

[[ -v PIONIER_ID ]] || {
    echo 
    echo '!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!';
    echo 'Please set the $PIONIER_ID environment variable.';
    echo '!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!';
    exit -1;
}
[[ -v ROS_NAMESPACE ]] || export ROS_NAMESPACE=/PIONIER$PIONIER_ID


echo
echo
echo "ENVIRONMENT CONFIG"
env | grep -e ROS_MASTER_URI -e ARIA_PORT -e PIONIER_ID -e ROS_NAMESPACE | sed 's/^/  - /'
echo
echo

set -x
rosrun rosaria RosAria _port:=$ARIA_PORT  _tf_prefix:=${ROS_NAMESPACE} &
rosrun hokuyo_node hokuyo_node _frame_id:="PIONIER${PIONIER_ID}/laser" &
rosrun tf static_transform_publisher 0.16 0 0 0 0 0 ${ROS_NAMESPACE}/base_link "${ROS_NAMESPACE}/laser" 100