added more collision avoidance tests

test/mpc_tracker/CMakeLists.txt

@@ -3,3 +3,7 @@ add_subdirectory(mpc_goto)
 add_subdirectory(mpc_avoidance_two_should_avoid)
 
 add_subdirectory(mpc_avoidance_two_should_crash)
+
+add_subdirectory(mpc_avoidance_two_uav1_passive)
+
+add_subdirectory(mpc_avoidance_two_uav2_passive)

test/mpc_tracker/mpc_avoidance_two_should_avoid/CMakeLists.txt

@@ -13,4 +13,16 @@ add_dependencies(test_${TEST_NAME}
   ${catkin_EXPORTED_TARGETS}
   )
 
-add_rostest(${TEST_NAME}.test)
+set(MODALITIES
+  attitude_rate
+  velocity_hdg
+  )
+
+foreach(MODALITY ${MODALITIES})
+
+  add_rostest(${TEST_NAME}.test
+    ARGS
+    modality:=${MODALITY}
+    )
+
+endforeach()

test/mpc_tracker/mpc_avoidance_two_should_avoid/config/hw_api_attitude_rate.yaml

@@ -0,0 +1,10 @@
+input_mode:
+  actuators:             false
+  control_group:         false
+  attitude_rate:         true
+  attitude:              false
+  acceleration_hdg_rate: false
+  acceleration_hdg:      false
+  velocity_hdg_rate:     false
+  velocity_hdg:          false
+  position:              false

test/mpc_tracker/mpc_avoidance_two_should_avoid/config/hw_api_velocity_hdg.yaml

@@ -0,0 +1,10 @@
+input_mode:
+  actuators:             false
+  control_group:         false
+  attitude_rate:         false
+  attitude:              false
+  acceleration_hdg_rate: false
+  acceleration_hdg:      false
+  velocity_hdg_rate:     false
+  velocity_hdg:          true
+  position:              false

test/mpc_tracker/mpc_avoidance_two_should_avoid/config/network_config.yaml

@@ -0,0 +1,16 @@
+# 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,
+    uav2,
+  ]

test/mpc_tracker/mpc_avoidance_two_should_avoid/mpc_avoidance_two_should_avoid.test

@@ -12,25 +12,28 @@
   <arg name="package_eval" default="eval(arg('import_eval') + '(\'rospkg\')').get_package_name(arg('this_path'))" />
   <arg name="package" default="$(eval eval(arg('package_eval')))" />
 
+  <arg name="modality" default="velocity_hdg" />
+
   <include file="$(find mrs_uav_testing)/launch/mrs_simulator.launch">
     <arg name="mrs_simulator_config" default="$(dirname)/config/mrs_simulator.yaml" />
   </include>
 
   <include file="$(find mrs_multirotor_simulator)/launch/hw_api.launch">
     <arg name="UAV_NAME" default="uav1" />
-    <arg name="custom_config" default="$(dirname)/config/hw_api.yaml" />
+    <arg name="custom_config" default="$(dirname)/config/hw_api_$(arg modality).yaml" />
   </include>
 
   <include file="$(find mrs_multirotor_simulator)/launch/hw_api.launch">
     <arg name="UAV_NAME" default="uav2" />
-    <arg name="custom_config" default="$(dirname)/config/hw_api.yaml" />
+    <arg name="custom_config" default="$(dirname)/config/hw_api_$(arg modality).yaml" />
   </include>
 
   <include file="$(find mrs_uav_testing)/launch/mrs_uav_system.launch">
     <arg name="automatic_start" default="false" />
     <arg name="platform_config" default="$(find mrs_multirotor_simulator)/config/mrs_uav_system/$(arg UAV_TYPE).yaml" />
     <arg name="custom_config" default="$(dirname)/config/custom_config.yaml" />
     <arg name="world_config" default="$(dirname)/config/world_config.yaml" />
+    <arg name="network_config" default="$(dirname)/config/network_config.yaml" />
     <arg name="UAV_NAME" default="uav1" />
   </include>
 
@@ -39,6 +42,7 @@
     <arg name="platform_config" default="$(find mrs_multirotor_simulator)/config/mrs_uav_system/$(arg UAV_TYPE).yaml" />
     <arg name="custom_config" default="$(dirname)/config/custom_config.yaml" />
     <arg name="world_config" default="$(dirname)/config/world_config.yaml" />
+    <arg name="network_config" default="$(dirname)/config/network_config.yaml" />
     <arg name="UAV_NAME" default="uav2" />
   </include>
 

test/mpc_tracker/mpc_avoidance_two_should_crash/config/network_config.yaml

@@ -0,0 +1,16 @@
+# 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,
+    uav2,
+  ]

test/mpc_tracker/mpc_avoidance_two_should_crash/mpc_avoidance_two_should_crash.test

@@ -31,6 +31,7 @@
     <arg name="platform_config" default="$(find mrs_multirotor_simulator)/config/mrs_uav_system/$(arg UAV_TYPE).yaml" />
     <arg name="custom_config" default="$(dirname)/config/custom_config.yaml" />
     <arg name="world_config" default="$(dirname)/config/world_config.yaml" />
+    <arg name="network_config" default="$(dirname)/config/network_config.yaml" />
     <arg name="UAV_NAME" default="uav1" />
   </include>
 
@@ -39,6 +40,7 @@
     <arg name="platform_config" default="$(find mrs_multirotor_simulator)/config/mrs_uav_system/$(arg UAV_TYPE).yaml" />
     <arg name="custom_config" default="$(dirname)/config/custom_config.yaml" />
     <arg name="world_config" default="$(dirname)/config/world_config.yaml" />
+    <arg name="network_config" default="$(dirname)/config/network_config.yaml" />
     <arg name="UAV_NAME" default="uav2" />
   </include>
 

test/mpc_tracker/mpc_avoidance_two_uav1_passive/CMakeLists.txt

@@ -0,0 +1,16 @@
+get_filename_component(TEST_NAME "${CMAKE_CURRENT_SOURCE_DIR}" NAME)
+
+catkin_add_executable_with_gtest(test_${TEST_NAME}
+  test.cpp
+  )
+
+target_link_libraries(test_${TEST_NAME}
+  ${catkin_LIBRARIES}
+  )
+
+add_dependencies(test_${TEST_NAME}
+  ${${PROJECT_NAME}_EXPORTED_TARGETS}
+  ${catkin_EXPORTED_TARGETS}
+  )
+
+add_rostest(${TEST_NAME}.test)

test/mpc_tracker/mpc_avoidance_two_uav1_passive/README.md

@@ -0,0 +1,4 @@
+* UAV1's avoidance is ON
+* UAV2's avoidance is OFF
+
+UAV1 should avoid despite its higher priority

test/mpc_tracker/mpc_avoidance_two_uav1_passive/config/custom_config_uav1.yaml

@@ -0,0 +1,32 @@
+# GET ALL PARAMETERS USABLE FOR CUSTOM CONFIG BY RUNNING:
+## --------------------------------------------------------------
+## |          rosrun mrs_uav_core get_public_params.py          #
+## --------------------------------------------------------------
+
+mrs_uav_managers:
+
+  estimation_manager:
+
+    # loaded state estimator plugins
+    state_estimators: [
+      "gps_baro",
+    ]
+
+    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)
+
+  uav_manager:
+
+    midair_activation:
+
+      after_activation:
+        controller: "Se3Controller"
+        tracker: "MpcTracker"
+
+mrs_uav_trackers:
+
+  mpc_tracker:
+
+    collision_avoidance:
+
+      enabled: true

test/mpc_tracker/mpc_avoidance_two_uav1_passive/config/custom_config_uav2.yaml

@@ -0,0 +1,32 @@
+# GET ALL PARAMETERS USABLE FOR CUSTOM CONFIG BY RUNNING:
+## --------------------------------------------------------------
+## |          rosrun mrs_uav_core get_public_params.py          #
+## --------------------------------------------------------------
+
+mrs_uav_managers:
+
+  estimation_manager:
+
+    # loaded state estimator plugins
+    state_estimators: [
+      "gps_baro",
+    ]
+
+    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)
+
+  uav_manager:
+
+    midair_activation:
+
+      after_activation:
+        controller: "Se3Controller"
+        tracker: "MpcTracker"
+
+mrs_uav_trackers:
+
+  mpc_tracker:
+
+    collision_avoidance:
+
+      enabled: false

test/mpc_tracker/mpc_avoidance_two_uav1_passive/config/mrs_simulator.yaml

@@ -0,0 +1,27 @@
+collisions:
+  enabled: false
+  crash: true
+
+individual_takeoff_platform:
+  enabled: true
+
+uav_names: [
+  "uav1",
+  "uav2",
+]
+
+uav1:
+  type: "x500"
+  spawn:
+    x: 0.0
+    y: 0.0
+    z: 5.0
+    heading: 0.0
+
+uav2:
+  type: "x500"
+  spawn:
+    x: 5.0
+    y: 0.0
+    z: 5.0
+    heading: 0.0

test/mpc_tracker/mpc_avoidance_two_uav1_passive/config/network_config.yaml

@@ -0,0 +1,16 @@
+# 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,
+    uav2,
+  ]

test/mpc_tracker/mpc_avoidance_two_uav1_passive/config/world_config.yaml

@@ -0,0 +1,34 @@
+world_origin:
+
+  units: "LATLON" # {"UTM, "LATLON"}
+
+  origin_x: 47.397743
+  origin_y: 8.545594
+
+safety_area:
+
+  enabled: false
+
+  horizontal:
+
+    # the frame of reference in which the points are expressed
+    frame_name: "world_origin"
+
+    # polygon
+    #
+    # x, y [m] for any frame_name except latlon_origin
+    # x = latitude, y = longitude [deg]  for frame_name=="latlon_origin"
+    points: [
+      -50, -50,
+      50,  -50,
+      50,  50,
+      -50, 50,
+    ]
+
+  vertical:
+
+    # the frame of reference in which the max&min z is expressed
+    frame_name: "world_origin"
+
+    max_z: 15.0
+    min_z: -2.0

test/mpc_tracker/mpc_avoidance_two_uav1_passive/mpc_avoidance_two_uav1_passive.test

@@ -0,0 +1,51 @@
+<launch>
+
+  <arg name="this_path" default="$(dirname)" />
+
+  <arg name="UAV_TYPE" default="x500" />
+
+    <!-- automatically deduce the test name -->
+  <arg name="test_name" default="$(eval arg('this_path').split('/')[-1])" />
+
+    <!-- automatically deduce the package name -->
+  <arg name="import_eval" default="eval('_' + '_import_' + '_')"/>
+  <arg name="package_eval" default="eval(arg('import_eval') + '(\'rospkg\')').get_package_name(arg('this_path'))" />
+  <arg name="package" default="$(eval eval(arg('package_eval')))" />
+
+  <include file="$(find mrs_uav_testing)/launch/mrs_simulator.launch">
+    <arg name="mrs_simulator_config" default="$(dirname)/config/mrs_simulator.yaml" />
+  </include>
+
+  <include file="$(find mrs_multirotor_simulator)/launch/hw_api.launch">
+    <arg name="UAV_NAME" default="uav1" />
+    <arg name="custom_config" default="$(dirname)/config/hw_api.yaml" />
+  </include>
+
+  <include file="$(find mrs_multirotor_simulator)/launch/hw_api.launch">
+    <arg name="UAV_NAME" default="uav2" />
+    <arg name="custom_config" default="$(dirname)/config/hw_api.yaml" />
+  </include>
+
+  <include file="$(find mrs_uav_testing)/launch/mrs_uav_system.launch">
+    <arg name="automatic_start" default="false" />
+    <arg name="platform_config" default="$(find mrs_multirotor_simulator)/config/mrs_uav_system/$(arg UAV_TYPE).yaml" />
+    <arg name="custom_config" default="$(dirname)/config/custom_config_uav1.yaml" />
+    <arg name="world_config" default="$(dirname)/config/world_config.yaml" />
+    <arg name="network_config" default="$(dirname)/config/network_config.yaml" />
+    <arg name="UAV_NAME" default="uav1" />
+  </include>
+
+  <include file="$(find mrs_uav_testing)/launch/mrs_uav_system.launch">
+    <arg name="automatic_start" default="false" />
+    <arg name="platform_config" default="$(find mrs_multirotor_simulator)/config/mrs_uav_system/$(arg UAV_TYPE).yaml" />
+    <arg name="custom_config" default="$(dirname)/config/custom_config_uav2.yaml" />
+    <arg name="world_config" default="$(dirname)/config/world_config.yaml" />
+    <arg name="network_config" default="$(dirname)/config/network_config.yaml" />
+    <arg name="UAV_NAME" default="uav2" />
+  </include>
+
+  <test pkg="$(arg package)" type="test_$(arg test_name)" test-name="$(arg test_name)" time-limit="60.0">
+    <param name="test" value="$(arg test_name)" />
+  </test>
+
+</launch>