Merge pull request #12 from mascheiber/dev/v1.1

Development Version 1.1 Update

.devcontainer/development/devcontainer.json

@@ -0,0 +1,26 @@
+{
+  "name": "Development Container",
+  "image": "aaucns/flightstack-vscode:dev",
+  "runArgs": [
+    "--privileged",
+    "--network=host",
+    "--pid=host"
+  ],
+  "containerEnv": {
+    "DISPLAY": "${localEnv:DISPLAY}"
+  },
+  "workspaceMount": "source=${localWorkspaceFolder},target=/home/vscode/${localWorkspaceFolderBasename},type=bind",
+  "workspaceFolder": "/home/vscode/${localWorkspaceFolderBasename}",
+  "mounts": [
+    "source=/tmp/.X11-unix,target=/tmp/.X11-unix,type=bind",
+    "source=${localEnv:HOME}${localEnv:USERPROFILE}/.bash_aliases,target=/home/vscode/.bash_aliases,type=bind",
+    "source=${localEnv:HOME}${localEnv:USERPROFILE}/.ssh,target=/home/vscode/.ssh,type=bind"
+  ],
+  "customizations": {
+    "vscode": {
+      "extensions": [
+        "ms-vscode.cpptools-extension-pack"
+      ]
+    }
+  }
+}

.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

.vscode/c_cpp_properties.json

@@ -0,0 +1,24 @@
+{
+    "configurations": [
+        {
+            "browse": {
+                "databaseFilename": "${default}",
+                "limitSymbolsToIncludedHeaders": false
+            },
+            "includePath": [
+                "/opt/ros/noetic/include/",
+                "/usr/include/",
+                "${workspaceFolder}/devel/include",
+                "${workspaceFolder}/build/",
+                "${workspaceFolder}/src/**/include/"
+            ],
+            "name": "ROS",
+            "intelliSenseMode": "gcc-x64",
+            "compilerPath": "/usr/bin/gcc",
+            "cStandard": "gnu11",
+            "cppStandard": "c++17",
+            "configurationProvider": "b2.catkin_tools"
+        }
+    ],
+    "version": 4
+}

.vscode/settings.json

@@ -0,0 +1,74 @@
+{
+  "files.associations": {
+    "cctype": "cpp",
+    "cstddef": "cpp",
+    "cstdlib": "cpp",
+    "cstring": "cpp",
+    "cwchar": "cpp",
+    "cwctype": "cpp",
+    "array": "cpp",
+    "atomic": "cpp",
+    "bit": "cpp",
+    "*.tcc": "cpp",
+    "compare": "cpp",
+    "concepts": "cpp",
+    "cstdint": "cpp",
+    "map": "cpp",
+    "set": "cpp",
+    "unordered_map": "cpp",
+    "unordered_set": "cpp",
+    "vector": "cpp",
+    "exception": "cpp",
+    "algorithm": "cpp",
+    "functional": "cpp",
+    "memory": "cpp",
+    "memory_resource": "cpp",
+    "random": "cpp",
+    "string": "cpp",
+    "string_view": "cpp",
+    "tuple": "cpp",
+    "type_traits": "cpp",
+    "utility": "cpp",
+    "initializer_list": "cpp",
+    "iosfwd": "cpp",
+    "iostream": "cpp",
+    "istream": "cpp",
+    "new": "cpp",
+    "ostream": "cpp",
+    "sstream": "cpp",
+    "stdexcept": "cpp",
+    "streambuf": "cpp",
+    "typeinfo": "cpp",
+    "strstream": "cpp",
+    "bitset": "cpp",
+    "chrono": "cpp",
+    "clocale": "cpp",
+    "cmath": "cpp",
+    "complex": "cpp",
+    "condition_variable": "cpp",
+    "cstdarg": "cpp",
+    "cstdio": "cpp",
+    "ctime": "cpp",
+    "deque": "cpp",
+    "list": "cpp",
+    "fstream": "cpp",
+    "iomanip": "cpp",
+    "limits": "cpp",
+    "mutex": "cpp",
+    "optional": "cpp",
+    "ratio": "cpp",
+    "system_error": "cpp",
+    "thread": "cpp",
+    "typeindex": "cpp",
+    "variant": "cpp"
+  },
+  "python.autoComplete.extraPaths": [
+    "${workspaceFolder}/devel/lib/python3/dist-packages",
+    "/opt/ros/noetic/lib/python3/dist-packages"
+  ],
+  "python.analysis.extraPaths": [
+    "${workspaceFolder}/devel/lib/python3/dist-packages",
+    "/opt/ros/noetic/lib/python3/dist-packages"
+  ],
+  "cmake.configureOnOpen": false
+}

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"
 
@@ -14,7 +18,7 @@ export FS_RECORD_ADD_DEV1_SENSOR=()
 export FS_RECORD_ADD_DEV1_CAM=()
 # e.g. these topics require the installation of the matrixvision driver
 # see https://github.com/ethz-asl/matrixvision_camera
-# export FS_RECORD_ADD_DEV1_CMAS=(
+# export FS_RECORD_ADD_DEV1_CAM=(
 # "/camera/image_raw"
 # "/camera/camera_info"
 # )

src/flightstack/flightstack_bringup/launch/fs_estimation.launch

@@ -14,6 +14,11 @@ You can contact the authors at <[email protected]>, <christian.brommer@ie
 
 <launch>
   <arg name="dev_id"                    default="1" />
+  <arg name="ros_ns"                    default="" />
+
+  <!-- topic prefix for namespace -->
+  <arg name="topic_prefix"              value="/$(arg ros_ns)" if="$(eval arg('ros_ns') != '')" />
+  <arg name="topic_prefix"              value="" unless="$(eval arg('ros_ns') != '')" />
 
   <arg name="optitrack_object_name"     default="$(env FS_OPTITRACK_OBJECT_NAME)" />
 
@@ -22,12 +27,12 @@ You can contact the authors at <[email protected]>, <christian.brommer@ie
   <arg name="use_px4_bridge"            default="False" />
 
   <!-- MaRS arguments -->
-  <arg name="mars_imu_in"               default="/mavros/imu/data_raw" />
-  <arg name="mars_transform_in"         default="/$(arg optitrack_object_name)/vrpn_client/raw_transform" />
-  <arg name="mars_pose_out"             default="/mavros/vision_pose/pose" />
-  <arg name="mars_lite_ext_state_out"   default="/mavros/external_state_lite/external_state_estimate_lite" />
-  <arg name="mars_gps_pos_in"           default="/mavros/global_position/raw/fix" />
-  <arg name="mars_gps_vel_in"           default="/mavros/global_position/raw/gps_vel" />
+  <arg name="mars_imu_in"               default="$(arg topic_prefix)/mavros/imu/data_raw" />
+  <arg name="mars_transform_in"         default="$(arg topic_prefix)/$(arg optitrack_object_name)/vrpn_client/raw_transform" />
+  <arg name="mars_pose_out"             default="$(arg topic_prefix)/mavros/vision_pose/pose" />
+  <arg name="mars_lite_ext_state_out"   default="$(arg topic_prefix)/mavros/external_state_lite/external_state_estimate_lite" />
+  <arg name="mars_gps_pos_in"           default="$(arg topic_prefix)/mavros/global_position/raw/fix" />
+  <arg name="mars_gps_vel_in"           default="$(arg topic_prefix)/mavros/global_position/raw/gps_vel" />
 
 
   <arg name="mars_config_in"            default="$(find mars_ros)/launch/config/pose_config.yaml" unless="$(arg use_gps)" />

src/flightstack/flightstack_bringup/launch/fs_namespace.launch

@@ -0,0 +1,83 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<!--
+  Copyright (C) 2023 Martin Scheiber,
+  Control of Networked Systems Group, University of Klagenfurt, Austria.
+  
+  All rights reserved.
+  
+  This software is licensed under the terms of the BSD-2-Clause-License with
+  no commercial use allowed, the full terms of which are made available
+  in the LICENSE file. No license in patents is granted.
+  
+  You can contact the author at <[email protected]>
+-->
+<launch>
+  <arg name="dev_id"            default="1" />
+  <arg name="ros_namespace"     default="cnsuav" />
+
+  <!-- parameters required for launching with fs_dev scripts -->
+  <arg name="dev_type"          default="xu4" />
+  <arg name="use_gps"           default="False" />
+  <arg name="rec_script_file"   default="$(find flightstack_scripts)/record_scripts/record_full.sh" />
+  <arg name="store_script_file" default="$(find flightstack_scripts)/store_scripts/safe_merge_data_single_dev.sh" />
+  <arg name="estimator_init_service_name" default="/$(arg ros_namespace)/$(env FS_ESTIMATOR_NODE_NAME)/init_service" />
+  <arg name="config_filepath"             default="$(find flightstack_bringup)/configs/autonomy/config_dh.yaml" />
+
+  <arg name="launch_file"       default="all" />
+  <!-- choose between 'all', 'estimation', 'navigation', 'operator', 'recording', 'saftey', and 'sensors'-->
+
+  <group ns="$(arg ros_namespace)">
+    <!-- ESTIMATION -->
+    <include file="$(find flightstack_bringup)/launch/fs_estimation.launch" 
+              if="$(eval arg('launch_file') == 'all' or arg('launch_file') == 'estimation')">
+      <arg name="dev_id"          value="$(arg dev_id)" />
+      <arg name="ros_ns"          value="$(arg ros_namespace)" />
+
+      <arg name="use_gps"         value="$(arg use_gps)" />
+    </include>
+
+    <!-- NAVIGATION -->
+    <include file="$(find flightstack_bringup)/launch/fs_navigation.launch" 
+              if="$(eval arg('launch_file') == 'all' or arg('launch_file') == 'navigation')">
+      <arg name="dev_id"          value="$(arg dev_id)" />
+      <arg name="ros_ns"          value="$(arg ros_namespace)" />
+
+      <arg name="use_gps"         value="$(arg use_gps)" />
+    </include>
+
+    <!-- OPERATOR -->
+    <include file="$(find flightstack_bringup)/launch/fs_operator.launch" 
+              if="$(eval arg('launch_file') == 'all' or arg('launch_file') == 'operator')">
+      <arg name="dev_id"          value="$(arg dev_id)" />
+      <arg name="ros_ns"          value="$(arg ros_namespace)" />
+
+      <arg name="estimator_init_service_name" value="$(arg estimator_init_service_name)" />
+      <arg name="config_filepath" value="$(arg config_filepath)" />
+    </include>
+
+    <!-- RECORDING -->
+    <include file="$(find flightstack_bringup)/launch/fs_recording.launch" 
+              if="$(eval arg('launch_file') == 'all' or arg('launch_file') == 'recording')">
+      <arg name="dev_id"          value="$(arg dev_id)" />
+      <arg name="ros_ns"          value="$(arg ros_namespace)" />
+    </include>
+
+    <!-- SAFETY -->
+    <include file="$(find flightstack_bringup)/launch/fs_safety.launch" 
+              if="$(eval arg('launch_file') == 'all' or arg('launch_file') == 'safety')">
+      <arg name="dev_id"          value="$(arg dev_id)" />
+      <arg name="ros_ns"          value="$(arg ros_namespace)" />
+
+      <arg name="use_gps"         value="$(arg use_gps)" />
+    </include>
+
+    <!-- SENSORS -->
+    <include file="$(find flightstack_bringup)/launch/fs_sensors.launch" 
+              if="$(eval arg('launch_file') == 'all' or arg('launch_file') == 'sensors')">
+      <arg name="dev_id"          value="$(arg dev_id)" />
+      <arg name="ros_ns"          value="$(arg ros_namespace)" />
+
+      <arg name="use_gps"         value="$(arg use_gps)" />
+    </include>
+  </group>
+</launch>

src/flightstack/flightstack_bringup/launch/fs_navigation.launch

@@ -14,6 +14,11 @@ You can contact the authors at <[email protected]>, <christian.brommer@ie
 
 <launch>
   <arg name="dev_id"        default="1" />
+  <arg name="ros_ns" default="" />
+
+  <!-- topic prefix for namespace -->
+  <arg name="topic_prefix" value="/$(arg ros_ns)" if="$(eval arg('ros_ns') != '')" />
+  <arg name="topic_prefix" value="" unless="$(eval arg('ros_ns') != '')" />
 
   <!-- system args -->
   <arg name="use_gps"               default="False" /> <!-- INFO(scm): kept for legacy reasons -->
@@ -37,8 +42,21 @@ You can contact the authors at <[email protected]>, <christian.brommer@ie
 
   <!-- odometry in -->
   <arg name="use_odom"                        default="True" />
-  <arg name="topic_odom_in"                   default="/$(env FS_ESTIMATOR_NODE_NAME)/odom_state_out" />
-  <arg name="topic_pose_in"                   default="/$(env FS_ESTIMATOR_NODE_NAME)/pose_state_out" />
+  <arg name="topic_odom_in"                   default="$(arg topic_prefix)/$(env FS_ESTIMATOR_NODE_NAME)/odom_state_out" />
+  <arg name="topic_pose_in"                   default="$(arg topic_prefix)/$(env FS_ESTIMATOR_NODE_NAME)/pose_state_out" />
+
+  <!-- mavros topics and services (for NS remapping) - value-based only -->
+  <arg name="ms_mavros_topic_state_in"          value="$(arg topic_prefix)/mavros/state" />
+  <arg name="ms_mavros_topic_extended_state_in" value="$(arg topic_prefix)/mavros/extended_state" />
+  <arg name="ms_mavros_topic_local_pose_in"     value="$(arg topic_prefix)/mavros/local_position/pose" />
+  <arg name="ms_autonomy_topic_request_in"      value="$(arg topic_prefix)/autonomy/request" />
+  <arg name="ms_mavros_topic_setpoint_out"      value="$(arg topic_prefix)/mavros/setpoint_position/local" />
+  <arg name="ms_autonomy_topic_response_out"    value="$(arg topic_prefix)/autonomy/response" />
+  <arg name="ms_mavros_service_arming_out"      value="$(arg topic_prefix)/mavros/cmd/arming" />
+  <arg name="ms_mavros_service_command_out"     value="$(arg topic_prefix)/mavros/cmd/command" />
+  <arg name="ms_mavros_service_land_out"        value="$(arg topic_prefix)/mavros/cmd/land" />
+  <arg name="ms_mavros_service_set_mode_out"    value="$(arg topic_prefix)/mavros/set_mode" />
+  <!-- TODO(scm): might not be required as PHN/NH difference already sets these values correctly -->
 
   <group if="$(eval arg('dev_id') == 1)">
     <include file="$(find mission_sequencer)/launch/mission_sequencer.launch">