added robofly stuff

config/worlds/world_cisar.yaml

@@ -65,5 +65,5 @@ safety_area:
     # the frame of reference in which the max&min z is expressed
     frame_name: "world_origin"
 
-    max_z: 8.0
+    max_z: 25.0
     min_z: 1.0

config/worlds/world_kn_yard.yaml

@@ -30,5 +30,5 @@ safety_area:
     # the frame of reference in which the max&min z is expressed
     frame_name: "world_origin"
 
-    max_z: 4.0
+    max_z: 8.0
     min_z: 1.0

tmux/robofly/config/custom_config.yaml

@@ -0,0 +1,289 @@
+mrs_uav_managers:
+
+  control_manager:
+    Se3Controller:
+      eland_threshold: 2.5
+      failsafe_threshold: 3.5
+
+  constraint_manager:
+
+    estimator_types: [
+      "open_vins",
+			"gps_baro",
+    ]
+
+    constraints: [
+      "slow",
+      "medium",
+      "fast"
+    ]
+
+    # list of allowed gains per odometry mode
+    allowed_constraints:
+      gps_baro: ["slow", "medium", "fast"]
+      open_vins: ["slow", "medium", "fast"]
+
+    constraints: [
+      "slow",
+      "medium",
+      "fast",
+    ]
+
+    allowed_constraints:
+      gps_baro: ["slow", "medium", "fast"]
+
+    default_constraints:
+      gps_baro: "medium"
+      open_vins: "medium"
+
+  estimation_manager:
+
+    # loaded state estimator plugins
+    # available in mrs_uav_state_estimators: gps_garmin, gps_baro, rtk, aloam, ground_truth, dummy
+    state_estimators: [
+    "gps_baro",
+    "open_vins",
+    ]
+
+    initial_state_estimator: "gps_baro" # will be used as the first state estimator
+    agl_height_estimator: "" # only slightly filtered height for checking min height (not used in control feedback)
+
+    gps_baro: # namespace of the state estimator
+
+      alt_baro: # namespace of the altitude estimator
+
+        process_noise: # process noise covariance (Q)
+          pos: 1.0 # position state
+          vel: 10.0 # velocity state
+          acc: 100.0 # acceleration state
+
+        corrections: [
+          # positional correction only makes the estimator unstable, vel_hw_api is not a derivative of pos_hw_api, vel_hw_api_only works best
+          "pos_hw_api"
+        ]
+
+        pos_hw_api:
+          state_id: 0 # 0 - position, 1 - velocity, 2 - acceleration
+          noise: 0.001 # measurement noise covariance (R)
+          noise_unhealthy_coeff: 100.0 # covariance gets multiplied by this coefficient when correction is unhealthy (R)
+          message:
+            type: "geometry_msgs/PointStamped"
+            topic: "hw_api/position" # without uav namespace
+            limit: 
+              delay: 1.0 # [s] messages with higher delay will flag correction as unhealthy
+              time_since_last: 0.5 # [s] larger time step between messages will flag correction as unhealthy
+
+          processors: [] # types of processors attached to this measurement
+
+    # namespace of the state estimator
+    open_vins:
+
+      address: "open_vins/OpenVinsEstimatorPlugin"
+
+      requires: # data required from the hw api
+        gnss: false
+        imu: false
+        distance_sensor: false
+        altitude: false
+        magnetometer_heading: false
+        position: false
+        orientation: true
+        velocity: true
+        angular_velocity: true
+
+      override_frame_id: # override the default frame_id with a custom one 
+        enabled: true
+        frame_id: "vio_origin" # the desired frame_id without the UAV namespace
+
+      estimators: # the names of the partial estimators
+        lateral:
+          name: "lat_open_vins"
+        altitude:
+          name: "alt_open_vins"
+          # name: "alt_garmin"
+        heading:
+          name: "hdg_open_vins"
+          passthrough: false # if true, then heading is not estimated but passed through from the orientation topic
+
+      topics:
+        orientation: "hw_api/orientation" # orientation passthrough
+        angular_velocity: "hw_api/angular_velocity" # angular velocity passthrough
+
+      # namespace of the altitude estimator
+      alt_open_vins: # namespace of the altitude estimator
+
+        max_flight_z: 100.0 # [m] maximum allowed flight Z (in the estimator frame)
+
+        innovation:
+          limit: 1.0 # [m] innovation limit that will trigger action
+          action: "switch" # {"eland", "switch", "mitigate"}
+
+        repredictor: # repredictor for correct fusion of delayed measurements
+          enabled: true
+          buffer_size: 200 # [samples] how many states and corrections are kept in history (i.e. estimator running at the default 100 Hz rate will be able to fuse corrections with up to 2 s delay with buffer size of 200 samples)
+
+        process_noise: # process noise covariance (Q)
+          pos: 1.0 # position state
+          vel: 100.0 # velocity state
+          acc: 100.0 # acceleration state
+
+        corrections: [
+          "pos_vio",
+          "vel_hw_api"
+        ]
+
+        pos_vio:
+          state_id: 0 # 0 - position, 1 - velocity, 2 - acceleration
+          noise: 0.001 # measurement noise covariance (R)
+          noise_unhealthy_coeff: 100.0 # covariance gets multiplied by this coefficient when correction is unhealthy (R)
+          message:
+            type: "nav_msgs/Odometry"
+            topic: "vins_republisher/odom" # without uav namespace
+            limit: 
+              delay: 1.0 # [s] messages with higher delay will flag correction as unhealthy
+              time_since_last: 0.5 # [s] larger time step between messages will flag correction as unhealthy
+
+          processors: [] # types of processors attached to this measurement
+
+        vel_hw_api:
+          state_id: 1 # 0 - position, 1 - velocity, 2 - acceleration
+          noise: 1.0 # measurement noise covariance (R)
+          noise_unhealthy_coeff: 100.0 # covariance gets multiplied by this coefficient when correction is unhealthy (R)
+          message:
+            type: "geometry_msgs/Vector3Stamped"
+            topic: "hw_api/velocity" # without uav namespace
+            limit: 
+              delay: 1.0 # [s] messages with higher delay will flag correction as unhealthy
+              time_since_last: 0.5 # [s] larger time step between messages will flag correction as unhealthy
+
+          processors: [] # types of processors attached to this measurement
+
+          median_filter:
+            buffer_size: 20 # the number of samples from which the median is calculated
+            max_diff: 2.0 # [m] reject measurement if difference from median is over this value
+
+
+      # namespace of the heading estimator
+      hdg_open_vins: # namespace of the heading estimator
+
+        max_flight_z: 100.0 # [m] maximum allowed flight Z (in the estimator frame)
+
+        position_innovation_limit: 1.0 # [rad] innovation limit that will trigger switch to other estimator
+
+        repredictor: # repredictor for correct fusion of delayed measurements
+          enabled: true
+          buffer_size: 200 # [samples] how many states and corrections are kept in history (i.e. estimator running at the default 100 Hz rate will be able to fuse corrections with up to 2 s delay with buffer size of 200 samples)
+
+        process_noise: # process noise covariance (Q)
+          pos: 0.1 # position state
+          vel: 0.1 # velocity state
+
+        corrections: [
+          "hdg_vio"
+        ]
+
+        hdg_vio:
+          state_id: 0 # 0 - position, 1 - velocity, 2 - acceleration
+          noise: 0.0001 # measurement noise covariance (R)
+          noise_unhealthy_coeff: 100.0 # covariance gets multiplied by this coefficient when correction is unhealthy (R)
+          message:
+            type: "nav_msgs/Odometry"
+            topic: "vins_republisher/odom" # without uav namespace
+            limit: 
+              delay: 1.0 # [s] messages with higher delay will flag correction as unhealthy
+              time_since_last: 0.5 # [s] larger time step between messages will flag correction as unhealthy
+
+          processors: [] # types of processors attached to this measurement
+
+      # namespace of the lateral estimator
+      lat_open_vins: # namespace of the lateral estimator
+
+        hdg_source_topic: "vio/hdg_vio/output" # [mrs_uav_state_estimation/EstimatorOutput]
+
+        max_flight_z: 100.0 # [m] maximum allowed flight Z (in the estimator frame)
+
+        innovation:
+          limit: 1.0 # [m] innovation limit that will trigger action
+          action: "eland" # {"eland", "switch", "mitigate"}
+
+        repredictor: # repredictor for correct fusion of delayed measurements
+          enabled: true
+          buffer_size: 200 # [samples] how many states and corrections are kept in history (i.e. estimator running at the default 100 Hz rate will be able to fuse corrections with up to 2 s delay with buffer size of 200 samples)
+
+        process_noise: # process noise covariance (Q)
+          pos: 0.1 # position state
+          vel: 1.0 # velocity state
+          acc: 1.0 # acceleration state
+
+        corrections: [
+          "pos_vio",
+          "vel_vio"
+        ]
+
+        pos_vio:
+          state_id: 0 # 0 - position, 1 - velocity, 2 - acceleration
+          noise: 0.01 # measurement noise covariance (R)
+          noise_unhealthy_coeff: 100.0 # covariance gets multiplied by this coefficient when correction is unhealthy (R)
+          message:
+            type: "nav_msgs/Odometry"
+            topic: "vins_republisher/odom" # without uav namespace
+            limit: 
+              delay: 1.0 # [s] messages with higher delay will flag correction as unhealthy
+              time_since_last: 0.5 # [s] larger time step between messages will flag correction as unhealthy
+
+          processors: [] # types of processors attached to this measurement
+
+        vel_vio:
+          state_id: 1 # 0 - position, 1 - velocity, 2 - acceleration
+          noise: 0.01 # measurement noise covariance (R)
+          noise_unhealthy_coeff: 100.0 # covariance gets multiplied by this coefficient when correction is unhealthy (R)
+          body_frame: true # velocity is in body frame from open_vins
+          message:
+            type: "nav_msgs/Odometry"
+            topic: "vins_republisher/odom" # without uav namespace
+            limit: 
+              delay: 1.0 # [s] messages with higher delay will flag correction as unhealthy
+              time_since_last: 0.5 # [s] larger time step between messages will flag correction as unhealthy
+
+          processors: [] # types of processors attached to this measurement
+
+  uav_manager:
+
+    takeoff:
+
+      during_takeoff:
+
+        controller: "MpcController"
+
+      after_takeoff:
+
+        controller: "MpcController"
+
+      takeoff_height: 0.8
+
+    midair_activation:
+      after_activation:
+        controller: "MpcController"
+
+  transform_manager:
+
+    open_vins:
+      odom_topic: "odom" # name of the topic (expects nav_msgs/Odometry topic type)
+      tf_from_attitude: # used for transforming velocities before full transform is available
+        enabled: true
+        attitude_topic: "attitude" # name of the attitude topic(expects geometry_msgs/QuaternionStamped topic type)
+      namespace: "estimation_manager/open_vins" # the namespace of the topic (usually the node that publishes the topic)
+      utm_based: false # if true, will publish tf to utm_origin
+      inverted: true # publish as inverted tf (the default for inverted mrs tf tree convention)
+      republish_in_frames: [] # the odometry message will be transformed and republished in the specified frames
+
+mrs_uav_trackers:
+
+  landoff_tracker:
+
+    max_position_difference: 2.0
+    vertical_tracker:
+
+      takeoff_speed: 1.0
+      takeoff_acceleration: 0.5
+

tmux/robofly/config/network_config.yaml

@@ -0,0 +1,15 @@
+# 1. This list is used by NimbroNetwork for mutual communication of the UAVs
+#    The names of the robots have to match hostnames described in /etc/hosts.
+#
+# 2. This list is used by MpcTracker for mutual collision avoidance of the UAVs.
+#    The names should match the true "UAV_NAMES" (the topic prefixes).
+#
+#     network_config:=~/config/network_config.yaml
+#
+#    to the core.launch and nimbro.launch.
+
+network:
+
+  robot_names: [
+    uav1,
+  ]

tmux/robofly/record.sh

@@ -0,0 +1,65 @@
+#!/bin/bash
+
+path="/home/\$(optenv USER mrs)/bag_files/latest/"
+
+# By default, we record everything.
+# Except for this list of EXCLUDED topics:
+exclude=(
+
+# IN GENERAL, DON'T RECORD CAMERAS
+#
+# If you want to record cameras, create a copy of this script
+# and place it at your tmux session.
+#
+# Please, seek an advice of a senior researcher of MRS about
+# what can be recorded. Recording too much data can lead to
+# ROS communication hiccups, which can lead to eland, failsafe
+# or just a CRASH.
+
+# Every topic containing "compressed"
+# '(.*)compressed(.*)'
+# Every topic containing "image_raw"
+# '(.*)image_raw(.*)'
+# Every topic containing "theora"
+# '(.*)theora(.*)'
+# Every topic containing "h264"
+# '(.*)h264(.*)'
+
+)
+
+# file's header
+filename=`mktemp`
+echo "<launch>" > "$filename"
+echo "<arg name=\"UAV_NAME\" default=\"\$(env UAV_NAME)\" />" >> "$filename"
+echo "<group ns=\"\$(arg UAV_NAME)\">" >> "$filename"
+
+echo -n "<node pkg=\"rosbag\" type=\"record\" name=\"rosbag_record\" output=\"screen\" args=\"-o $path -a" >> "$filename"
+
+# if there is anything to exclude
+if [ "${#exclude[*]}" -gt 0 ]; then
+
+  echo -n " -x " >> "$filename"
+
+  # list all the string and separate the with |
+  for ((i=0; i < ${#exclude[*]}; i++));
+  do
+    echo -n "${exclude[$i]}" >> "$filename"
+    if [ "$i" -lt "$( expr ${#exclude[*]} - 1)" ]; then
+      echo -n "|" >> "$filename"
+    fi
+  done
+
+fi
+
+echo "\">" >> "$filename"
+
+echo "<remap from=\"~status_msg_out\" to=\"mrs_uav_status/display_string\" />" >> "$filename"
+echo "<remap from=\"~data_rate_out\" to=\"~data_rate_MB_per_s\" />" >> "$filename"
+
+# file's footer
+echo "</node>" >> "$filename"
+echo "</group>" >> "$filename"
+echo "</launch>" >> "$filename"
+
+cat $filename
+roslaunch $filename