add: remapping for pos. ctrl based on optitrack

src/flightstack/flightstack_bringup/launch/fs_sensors.launch

@@ -27,9 +27,11 @@ You can contact the authors at <[email protected]>, <christian.brommer@ie
   <arg name="topic_prefix"  value="" unless="$(eval arg('ros_ns') != '')" />
 
   <!-- Optitrack Arguments -->
-  <arg name="use_gps"               default="False" />
-  <arg name="optitrack_object_name" default="$(env FS_OPTITRACK_OBJECT_NAME)" />
-  <arg name="optitrack_throttle"    default="False" />
+  <arg name="use_gps"                 default="False" />
+  <arg name="optitrack_object_name"   default="$(env FS_OPTITRACK_OBJECT_NAME)" />
+  <arg name="optitrack_throttle"      default="False" />
+  <arg name="optitrack_throttle_rate" default="30" />
+  <arg name="optitrack_remap_to_ctrl" default="$(arg optitrack_throttle)" />
 
   <!-- Device 1 -->
   <group if="$(eval arg('dev_id') == 1)">
@@ -45,6 +47,8 @@ You can contact the authors at <[email protected]>, <christian.brommer@ie
     <include file="$(find flightstack_bringup)/launch/individual/optitrack_aau_dronehall.launch" unless="$(arg use_gps)">
       <arg name="object_name"       value="$(arg optitrack_object_name)" />
       <arg name="throttle"          value="$(arg optitrack_throttle)" />
+      <arg name="throttle_rate"     value="$(arg optitrack_throttle_rate)" />
+      <arg name="remap_to_ctrl"     value="$(arg optitrack_remap_to_ctrl)" />
     </include>
   </group>
 

src/flightstack/flightstack_bringup/launch/individual/fs_posctrl.launch

@@ -0,0 +1,51 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<!--
+Copyright (C) 2022-2024 Martin Scheiber, Christian Brommer,
+and others, Control of Networked Systems, 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 authors at <[email protected]>, <[email protected]>.
+-->
+
+<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="px4_config_file"       default="$(find mavros)/launch/px4_config.yaml" />
+  <arg name="px4_pluginlists_file"  default="$(find mavros)/launch/px4_pluginlists.yaml" />
+  <arg name="px4_autorestart"       default="False" />
+  <arg name="ros_ns"                default="" />
+
+  <!-- Optitrack Arguments -->
+  <arg name="use_gps"                 default="False" />
+  <arg name="optitrack_object_name"   default="$(env FS_OPTITRACK_OBJECT_NAME)" />
+  <arg name="optitrack_throttle"      default="False" />
+  <arg name="optitrack_throttle_rate" default="30" />
+
+  <!-- Device 1 -->
+  <group if="$(eval arg('dev_id') == 1)">
+    <include file="$(find flightstack_bringup)/launch/fs_sensors.launch">
+      <arg name="dev_id"                  value="$(arg value)" />
+      <arg name="dev_type"                value="$(arg dev_type)" />
+      <arg name="dev_port"                value="$(arg dev_port)" />
+      <arg name="dev_baud"                value="$(arg dev_baud)" />
+      <arg name="px4_config_file"         value="$(arg px4_config_file)" />
+      <arg name="px4_pluginlists_file"    value="$(arg px4_pluginlists_file)" />
+      <arg name="px4_autorestart"         value="$(arg px4_autorestart)" />
+      <arg name="ros_ns"                  value="$(arg ros_ns)" />
+
+      <arg name="use_gps"                 value="$(arg use_gps)" />
+      <arg name="optitrack_object_name"   value="$(arg optitrack_object_name)" />
+      <arg name="optitrack_throttle"      value="$(arg optitrack_throttle)" />
+      <arg name="optitrack_throttle_rate" value="$(arg optitrack_throttle_rate)" />
+      <arg name="optitrack_remap_to_ctrl" value="True" />
+    </include>
+  </group>
+
+</launch>

src/flightstack/flightstack_bringup/launch/individual/optitrack_aau_dronehall.launch

@@ -18,8 +18,10 @@ and <[email protected]>.
 
   <!-- throttle options -->
   <arg name="throttle"            default="False" />
-  <arg name="throttle_in_topic"   default="/$(arg object_name)/vrpn_client/raw_pose" />
-  <arg name="throttle_out_topic"  default="/mavros/vision_pose/pose" />
+  <arg name="remap_to_ctrl"       default="$(arg throttle)" />
+  <arg name="throttle_in_topic"   default="/$(arg object_name)/vrpn_client/raw_pose_throttled" />
+  <arg name="throttle_out_topic"  default="/$(arg object_name)/vrpn_client/raw_pose" unless="$(arg remap_to_ctrl)" />
+  <arg name="throttle_out_topic"  default="/mavros/vision_pose/pose" if="$(arg remap_to_ctrl)" />
   <arg name="throttle_rate"       default="30" />
 
   <node ns="$(arg object_name)" name="vrpn_client" type="ros_vrpn_client" pkg="ros_vrpn_client" output="screen">
@@ -42,7 +44,8 @@ and <[email protected]>.
     <param name="rotational_estimator/output_minimal_quaternions" value="false" />
     <param name="rotational_estimator/output_minimal_quaternions" value="false" />
 
-    <remap from="/$(arg object_name)/vrpn_client/raw_pose" to="/mavros/vision_pose/pose" if="$(arg throttle)"/>
+    <remap from="/$(arg object_name)/vrpn_client/raw_pose" to="$(arg throttle_in_topic)" if="$(arg throttle)" />
+    <remap from="/$(arg object_name)/vrpn_client/raw_pose" to="/mavros/vision_pose/pose" if="$(eval arg('remap_to_ctrl') and not arg('throttle'))" />
   </node>
 
   <node

src/flightstack/flightstack_scripts/record_scripts/record_full.sh

@@ -211,6 +211,8 @@ est_topics=(
 "/mavros/local_position/pose_cov"
 "/mavros/local_position/odom"
 "/mavros/local_position/odom_cov"
+"/mavros/external_state/external_state"
+"/mavros/external_state_lite/external_state_lite"
 )
 
 mars_topics=(