Merge branch 'feat/launch_improvements' into development

.gitmodules

@@ -46,3 +46,6 @@
 [submodule "src/mavlink-gbp-release"]
 	path = src/mavlink-gbp-release
 	url = https://github.com/aau-cns/mavlink-gbp-release.git
+[submodule "src/mocap_bridge"]
+	path = src/mocap_bridge
+	url = https://github.com/aau-cns/mocap_bridge.git

scripts/create_bringup.sh

@@ -98,6 +98,15 @@ WORKSPACE_SRC=${PATH_WS}/${CONFIG_NAME}_cws/src
 PACKAGE_DIR=${PATH_WS}/${CONFIG_NAME}_cws/src/${CONFIG_NAME}
 BRINGUP_DIR=${PATH_WS}/${CONFIG_NAME}_cws/src/${CONFIG_NAME}/${CONFIG_NAME}_bringup
 
+# check if executed in correct folder
+if [ ! -d 'scripts' ]; then
+  echo "${COL_ERR}No 'scripts' folder found, please execute this script in the root directory of the flightstack_cws.${NC}"
+  exit 1;
+elif [ ! -f 'scripts/create_bringup.sh' ]
+  echo "${COL_ERR}No 'create_bringup' script found within the './scripts' folder, please execute this script in the root directory of the flightstack_cws.${NC}"
+  exit 1;
+fi
+
 # create directory and copy template
 mkdir -p ${PACKAGE_DIR}
 rsync -avPh $(pwd)/src/flightstack/template_config/ ${PACKAGE_DIR}

src/flightstack/flightstack_bringup/configs/global/fs_vars.env

@@ -1,5 +1,9 @@
 #!/usr/bin/env bash
 
+# path related variables
+export FS_SCRIPTS_PATH=$(rospack find flightstack_scripts)
+export FS_VARS_FILE=$(rospack find flightstack_bringup)/configs/global/fs_vars.env
+
 # object tracking related variables
 export FS_OPTITRACK_OBJECT_NAME="cnsuav"
 

src/flightstack/flightstack_bringup/launch/fs_operator.launch

@@ -22,21 +22,21 @@ You can contact the authors at <[email protected]>, <christian.brommer@ie
   <arg name="config_filepath"                         default="$(find flightstack_bringup)/configs/autonomy/config_dh.yaml" />
 
   <!-- autonomy topics -->
-  <arg name="watchdog_heartbeat_topic"                default="/watchdog/status" />
-  <arg name="watchdog_status_topic"                   default="/watchdog/log" />
-  <arg name="watchdog_action_topic"                   default="/watchdog/action" />
-  <arg name="mission_sequencer_request_topic"         default="/autonomy/request" />
-  <arg name="mission_sequencer_response_topic"        default="/autonomy/response" />
-  <arg name="landing_detection_topic"                 default="/toland_detector/is_landed" />
-  <arg name="mission_sequencer_waypoints_topic"       default="/mission_sequencer/waypoint_list" />
+  <arg name="watchdog_heartbeat_topic"                default="watchdog/status" />
+  <arg name="watchdog_status_topic"                   default="watchdog/log" />
+  <arg name="watchdog_action_topic"                   default="watchdog/action" />
+  <arg name="mission_sequencer_request_topic"         default="autonomy/request" />
+  <arg name="mission_sequencer_response_topic"        default="autonomy/response" />
+  <arg name="landing_detection_topic"                 default="toland_detector/is_landed" />
+  <arg name="mission_sequencer_waypoints_topic"       default="mission_sequencer/waypoint_list" />
 
   <!-- autonomy services -->
-  <arg name="watchdog_start_service_name"             default="/watchdog/service/start" />
-  <arg name="data_recrding_service_name"              default="/data_recorder/record" />
-  <arg name="takeoff_service_name"                    default="/toland_detector/service/takeoff" />
-  <arg name="estimator_supervisor_service_name"       default="/estimator_supervisor/service/supervision" />
-  <arg name="estimator_init_service_name"             default="/$(env FS_ESTIMATOR_NODE_NAME)/init_service" />
-  <arg name="inflight_sensor_init_services_name"      default="[/sensor/start]" />
+  <arg name="watchdog_start_service_name"             default="watchdog/service/start" />
+  <arg name="data_recrding_service_name"              default="data_recorder/record" />
+  <arg name="takeoff_service_name"                    default="toland_detector/service/takeoff" />
+  <arg name="estimator_supervisor_service_name"       default="estimator_supervisor/service/supervision" />
+  <arg name="estimator_init_service_name"             default="$(env FS_ESTIMATOR_NODE_NAME)/init_service" />
+  <arg name="inflight_sensor_init_services_name"      default="[sensor/start]" />
 
   <!-- other arguments -->
   <arg name="watchdog_rate_Hz"                        default="0.5" />

src/flightstack/flightstack_bringup/launch/fs_sensors.launch

@@ -1,6 +1,6 @@
 <?xml version="1.0" encoding="UTF-8"?>
 <!--
-Copyright (C) 2022 Martin Scheiber, Christian Brommer,
+Copyright (C) 2022-2023 Martin Scheiber, Christian Brommer,
 and others, Control of Networked Systems, University of Klagenfurt, Austria.
 
 All rights reserved.
@@ -13,10 +13,12 @@ You can contact the authors at <[email protected]>, <christian.brommer@ie
 -->
 
 <launch>
-  <arg name="dev_id"    default="1" />
-  <arg name="dev_type"  default="xu4" /> <!-- or "pi" -->
-  <arg name="dev_port"  default="" /> <!--ignored if empty -->
-  <arg name="dev_baud"  default="" /> <!--ignored if empty -->
+  <arg name="dev_id"                default="1" />
+  <arg name="dev_type"              default="xu4" /> <!-- or "pi" -->
+  <arg name="dev_port"              default="" /> <!--ignored if empty -->
+  <arg name="dev_baud"              default="" /> <!--ignored if empty -->
+  <arg name="px4_config_file"       default="$(find mavros)/launch/px4_config.yaml" />
+  <arg name="px4_pluginlists_file"  default="$(find mavros)/launch/px4_pluginlists.yaml" />
 
   <!-- Optitrack Arguments -->
   <arg name="use_gps"               default="False" />
@@ -29,6 +31,9 @@ You can contact the authors at <[email protected]>, <christian.brommer@ie
       <arg name="platform"          value="$(arg dev_type)" if="$(eval not arg('dev_port'))" />
       <arg name="device_id"         value="$(arg dev_port)" unless="$(eval not arg('dev_port'))"/>
       <arg name="conn_baudrate"     value="$(arg dev_baud)" unless="$(eval not arg('dev_baud'))"/>
+
+      <arg name="config_file"       value="$(arg px4_config_file)" />
+      <arg name="pluginlists_file"  value="$(arg px4_pluginlists_file)" />
     </include>
     <include file="$(find flightstack_bringup)/launch/individual/optitrack_aau_dronehall.launch" unless="$(arg use_gps)">
       <arg name="object_name"       value="$(arg optitrack_object_name)" />

src/flightstack/flightstack_bringup/launch/individual/px4.launch

@@ -1,6 +1,6 @@
 <?xml version="1.0" encoding="UTF-8"?>
 <!--
-Copyright (C) 2022 Martin Scheiber, Christian Brommer,
+Copyright (C) 2022-2023 Martin Scheiber, Christian Brommer,
 and others, Control of Networked Systems, University of Klagenfurt, Austria.
 
 All rights reserved.
@@ -19,10 +19,35 @@ You can contact the authors at <[email protected]>, <christian.brommer@ie
   <arg name="device_id"         default="SAC0"  if="$(eval arg('platform') == 'xu4')" />
   <arg name="device_id"         default="THS0"  if="$(eval arg('platform') == 'agx')" />
   <arg name="device_id"         default="USB0"  if="$(eval arg('platform') == 'pc')" />
+  <arg name="device_id"         default="udp://:[email protected]:14577"
+                                if="$(eval arg('platform') == 'sim')" />
 
   <arg name="conn_baudrate"     default="921600" />
 
-  <include file="$(find mavros)/launch/px4.launch">
-    <arg name="fcu_url" default="/dev/tty$(arg device_id):$(arg conn_baudrate)" />
+  <!-- default mavros parameters -->
+  <arg name="config_file"       default="$(find mavros)/launch/px4_config.yaml" />
+  <arg name="pluginlists_file"  default="$(find mavros)/launch/px4_pluginlists.yaml" />
+  <arg name="fcu_url"           default="/dev/tty$(arg device_id):$(arg conn_baudrate)" 
+                                  unless="$(eval arg('platform') == 'sim')" />
+  <arg name="fcu_url"           default="$(arg device_id)" 
+                                  if="$(eval arg('platform') == 'sim')" />
+  <arg name="gcs_url"           default="" />
+  <arg name="tgt_system"        default="1" />
+  <arg name="tgt_component"     default="1" />
+  <arg name="log_output"        default="screen" />
+  <arg name="fcu_protocol"      default="v2.0" />
+  <arg name="respawn_mavros"    default="false" />
+
+  <include file="$(find mavros)/launch/node.launch">
+    <arg name="pluginlists_yaml"  value="$(arg pluginlists_file)" />
+    <arg name="config_yaml"       value="$(arg config_file)" />
+
+    <arg name="fcu_url"           value="$(arg fcu_url)" />
+    <arg name="gcs_url"           value="$(arg gcs_url)" />
+    <arg name="tgt_system"        value="$(arg tgt_system)" />
+    <arg name="tgt_component"     value="$(arg tgt_component)" />
+    <arg name="log_output"        value="$(arg log_output)" />
+    <arg name="fcu_protocol"      value="$(arg fcu_protocol)" />
+    <arg name="respawn_mavros"    value="$(arg respawn_mavros)" />
   </include>
 </launch>

src/flightstack/flightstack_scripts/driver_scripts/rs_check.sh

@@ -9,20 +9,37 @@
 # in the LICENSE file. No license in patents is granted.
 #
 # You can contact the author at <[email protected]>
-# and <[email protected]>
+# and <[email protected]>.
 
 # this bash script adheres to linux exit codes
 # see https://tldp.org/LDP/abs/html/exitcodes.html for further information
 
-# echo "[BASH - RS] checking sensor - Realsense"
+RS_IS_REMOTE=false
+RS_DEV_IP="10.42.0.102"
+RS_DEV_USER="flightstack"
+
+
+if [ ${RS_IS_REMOTE} == true ]; then
+  # echo "[BASH - RS] checking sensor - Realsense"
+  RS_CHECK="$(ssh ${RS_DEV_USER}@${RS_DEV_IP} 'rs-enumerate-devices')"
+  # echo "[BASH - RS] status: ${RS_CHECK}"
+
+  # check if device is found in lsusb
+  RS_ID="8087:0b37"
+  RS_STATUS="$(ssh ${RS_DEV_USER}@${RS_DEV_IP} 'lsusb' | grep ${RS_ID})"
+  # echo "[BASH - RS] status: ${RS_STATUS}"
+else
+  # echo "[BASH - RS] checking sensor - Realsense"
+  RS_CHECK="$(rs-enumerate-devices)"
+  # echo "[BASH - RS] status: ${RS_CHECK}"
+
+  # check if device is found in lsusb
+  RS_ID="8087:0b37"
+  RS_STATUS="$(lsusb | grep ${RS_ID})"
+  # echo "[BASH - RS] status: ${RS_STATUS}"
+fi
 
-RS_CHECK="$(rs-enumerate-devices)"
-# echo "[BASH - RS] status: ${RS_CHECK}"
 
-# check if device is found in lsusb
-RS_ID="8087:0b37"
-RS_STATUS="$(ssh [email protected] 'lsusb' | grep ${RS_ID})"
-# echo "[BASH - RS] status: ${RS_STATUS}"
 
 case $RS_STATUS in
   "")

src/flightstack/flightstack_scripts/driver_scripts/rs_restart.sh

@@ -14,12 +14,22 @@
 # this bash script adheres to linux exit codes
 # see https://tldp.org/LDP/abs/html/exitcodes.html for further information
 
-# restart realsense hub
-# ssh [email protected] 'sudo uhubctl -a off --delay 2 -e -R -l2 -p1'
-# command by chris
-#ssh [email protected] 'sudo uhubctl --action cycle --location 2'
-ssh [email protected] 'sudo uhubctl --action cycle --location 2 -R --delay 5'
-sleep 5
-ssh [email protected] 'rs-enumerate-devices'
+RS_IS_REMOTE=false
+RS_DEV_IP="10.42.0.102"
+RS_DEV_USER="flightstack"
+
+if [ ${RS_IS_REMOTE} == true ]; then
+  # restart realsense hub
+  # ssh [email protected] 'sudo uhubctl -a off --delay 2 -e -R -l2 -p1'
+  # command by chris
+  #ssh [email protected] 'sudo uhubctl --action cycle --location 2'
+  ssh ${RS_DEV_USER}@${RS_DEV_IP} 'sudo uhubctl --action cycle --location 2 -R --delay 5'
+  sleep 5
+  ssh ${RS_DEV_USER}@${RS_DEV_IP} 'rs-enumerate-devices'
+else
+  sudo uhubctl --action cycle --location 2 -R --delay 5
+  sleep 5
+  rs-enumerate-devices
+fi
 
 exit 0

src/flightstack/flightstack_scripts/record_scripts/record_full.sh

@@ -48,11 +48,12 @@ path_media=""
 
 DEV1_CAM_NODELET_MANAGER="record_dev1_manager"
 DEV2_CAM_NODELET_MANAGER="record_dev2_manager"
-B_CAMS_START_MANAGER=true
+B_CAMS_START_MANAGER=false
 
 # booleans (flags)
 B_USE_ROSBAG_RECORD=false
 B_DEBUG_ON=false
+B_DRY_RUN=false
 
 ################################################################################
 # Help                                                                         #
@@ -75,7 +76,9 @@ print_help(){
     echo ""
     echo "    -b              use 'rosbag record' instead of the flight stack's"
     echo "                      'nodlet recorder'"
-    echo "    -c              use 'nodelet_manager' for standard recording settings"
+    echo "    -c              use 'nodelet_manager' for camera recording settings"
+    echo "    -s              enable starting of camera recording manager"
+    echo "    -n              dry-run"
     echo "    -v              enable detailed debug output"
     echo ""
     echo "    -h          print this help"
@@ -103,18 +106,20 @@ fi
 OPTIND=$((OPTIND+1));
 
 # parse flags
-while getopts hvbcl:m:p: flag
+while getopts hvsnbcl:m:p: flag
 do
   case "${flag}" in
     l) path_local=${OPTARG};;
     m) path_media=${OPTARG};;
     p) bag_name=${OPTARG};;
 
     c)  DEV1_CAM_NODELET_MANAGER="nodelet_manager";
-        DEV2_CAM_NODELET_MANAGER="nodelet_manager";
-        B_CAMS_START_MANAGER=false;;
+        DEV2_CAM_NODELET_MANAGER="nodelet_manager";;
+
+    s)  B_CAMS_START_MANAGER=true;;
 
     b) B_USE_ROSBAG_RECORD=true;;
+    n) B_DRY_RUN=true;;
     v) B_DEBUG_ON=true;;
     h) print_help;;
 
@@ -143,6 +148,12 @@ if [ ! -d "${path_media}" ]; then
   mkdir -p ${path_media}
 fi
 
+################################################################################
+# GET GLOBAL VARS                                                              #
+################################################################################
+
+source ${FS_VARS_FILE}
+
 ################################################################################
 # SETUP TOPIC GROUPS                                                           #
 ################################################################################
@@ -172,6 +183,8 @@ autonomy_topics=(
 
 ms_topics=(
 "/mission_sequencer/waypoint_list"
+"/mission_sequencer/get_waypoint_list"
+"/mission_sequencer/waypoint_reached"
 "/mavros/setpoint_position/local"
 "/mavros/setpoint_position/global"
 "/mavros/state"
@@ -219,6 +232,13 @@ ${px4_topics[@]}
 ${FS_RECORD_ADD_DEV1_SENSOR[@]}
 )
 
+if [ ${B_DEBUG_ON} == true ]; then
+  echo "sensor additions:  ${#FS_RECORD_ADD_DEV1_SENSOR[@]}"
+  for entry in ${FS_RECORD_ADD_DEV1_SENSOR[@]}; do
+    echo "${entry}"
+  done
+fi
+
 ### Nodes
 group_mod1_nodes=(
 ${autonomy_topics[@]}
@@ -234,6 +254,13 @@ group_mod1_cams=(
 ${FS_RECORD_ADD_DEV1_CAM[@]}
 )
 
+if [ ${B_DEBUG_ON} == true ]; then
+  echo "cam additions: ${#FS_RECORD_ADD_DEV1_CAM[@]}"
+  for entry in ${FS_RECORD_ADD_DEV1_CAM[@]}; do
+    echo "${entry}"
+  done
+fi
+
 
 # generate final list of topics for recording - dev1
 if [ ${B_USE_ROSBAG_RECORD} == true ]; then
@@ -503,7 +530,7 @@ else
       start_manager:=${B_CAMS_START_MANAGER}
       nodelet_manager_name:=${DEV1_CAM_NODELET_MANAGER}
       nodelet_name:=record_dev1_cams
-      rosbag_path:=${path_local} rosbag_prefix:=${bag_name}_mod1_cams
+      rosbag_path:=${path_media} rosbag_prefix:=${bag_name}_mod1_cams
       rosbag_topics:=[${topics_mod1_cams%,}]" )
   fi
 
@@ -528,10 +555,12 @@ echo ""
 
 # execute recording in bkg
 for cmd in "${RECORD_CMD_ARR[@]}"; do
-  if [ ${B_DEBUG_ON} == true ]; then
+  if [ ${B_DEBUG_ON} == true ] || [ ${B_DRY_RUN} == true ]; then
     echo -e "${COL_DEB}executing: ${cmd}${NC}"
   fi
-  ${cmd} &
+  if [ ${B_DRY_RUN} != true ]; then
+    ${cmd} &
+  fi
 done
 
 if [ ${B_DEBUG_ON} = true ]; then