wip on a field

docker/shared_data/gnss/shared_data/custom_config.yaml

@@ -9,3 +9,13 @@ mrs_uav_managers:
     ]
 
     initial_state_estimator: "gps_baro" # will be used as the first state estimator
+
+  uav_manager:
+
+    takeoff:
+
+      during_takeoff:
+        controller: "MpcController"
+
+      after_takeoff:
+        controller: "MpcController"

docker/shared_data/gnss/shared_data/precise_landing/apriltag.yaml

@@ -0,0 +1,24 @@
+# AprilTag 3 code parameters
+# Find descriptions in apriltag/include/apriltag.h:struct apriltag_detector
+#                      apriltag/include/apriltag.h:struct apriltag_family
+tag_family:        'tagCustom48h12' # options: tagStandard52h13, tagStandard41h12, tag36h11, tag25h9, tag16h5, tagCustom48h12, tagCircle21h7, tagCircle49h12
+tag_threads:       2          # default: 2
+tag_decimate:      1.0        # default: 1.0
+tag_blur:          0.0        # default: 0.0
+tag_refine_edges:  1          # default: 1
+tag_debug:         0          # default: 0
+max_hamming_dist:  2          # default: 2 (Tunable parameter with 2 being a good choice - values >=3 consume large amounts of memory. Choose the largest value possible.)
+# Other parameters
+publish_tf:        true       # default: false
+transport_hint:    "raw"      # default: raw, see http://wiki.ros.org/image_transport#Known_Transport_Packages for options
+
+# standalone_tags:
+#   [
+#     {id: ID, size: SIZE, name: NAME},
+#     ...
+#   ]
+standalone_tags:
+  [
+    {id: 0, size: 0.129, name: recursive_tag_1_big},
+    {id: 10, size: 0.0255, name: recursive_tag_1_small},
+  ]

docker/shared_data/gnss/shared_data/precise_landing/landing_controller.yaml

@@ -0,0 +1,23 @@
+desired_heading:
+
+  relative_to_pad:
+    enabled: false
+    heading: 0 # [rad]
+
+stages:
+
+  aligning:
+    speed: 0.2    # [m/s]
+    height: 1.3   # [m]
+
+  repeating:
+    height: 1.3 # [m]
+
+  aborting:
+    height: 1.3 # [m]
+
+  descending:
+    speed: 0.2    # [m/s]
+
+  landing:
+    speed: 0.2          # [m/s]

docker/shared_data/gnss/shared_data/precise_landing/landing_estimator.yaml

@@ -0,0 +1,6 @@
+# the list of tag ids order according to their priority (highest-to-lowest)
+tag_ids: [0, 10] # [-]
+
+# the estimation will happen in this frame of refernece
+# (detections are transformed to this frame first)
+estimation_frame: "fixed_origin"

docker/shared_data/gnss/shared_data/record.sh

@@ -19,6 +19,8 @@ exclude=(
 # don't record cameras
 '(.*)camera_(.*)'
 
+'(.*)apriltag(.*)'
+
 '(.*)ov_msckf(.*)'
 
 '(.*)mavros(.*)'

docker/shared_data/gnss/shared_data/world_temesvar_field.yaml

@@ -0,0 +1,38 @@
+world_origin:
+
+  units: "LATLON" # {"UTM, "LATLON"}
+
+  origin_x: 49.36225
+  origin_y: 14.26143
+
+safety_area:
+
+  enabled: true
+
+  horizontal:
+
+    # the frame of reference in which the points are expressed
+    frame_name: "latlon_origin"
+
+    # polygon
+    #
+    # x, y [m] for any frame_name except latlon_origin
+    # x = latitude, y = longitude [deg]  for frame_name=="latlon_origin"
+    points: [
+      49.36179, 14.26094,
+      49.36166, 14.26124,
+      49.36173, 14.26190,
+      49.36311, 14.26350,
+      49.36355, 14.26084,
+      49.36232, 14.26025,
+      49.362091, 14.260506,
+      49.361990, 14.260631,
+    ]
+
+  vertical:
+
+    # the frame of reference in which the max&min z is expressed
+    frame_name: "world_origin"
+
+    max_z: 20.0
+    min_z: -5.0

docker/shared_data/vio/shared_data/custom_config.yaml

@@ -6,7 +6,33 @@ mrs_uav_managers:
     # available in mrs_uav_state_estimators: gps_garmin, gps_baro, rtk, aloam, ground_truth, dummy
     state_estimators: [
     "open_vins",
-    # "vins_kickoff",
+    "vins_kickoff",
     ]
 
-    initial_state_estimator: "open_vins" # will be used as the first state estimator
+    initial_state_estimator: "vins_kickoff" # will be used as the first state estimator
+
+  uav_manager:
+
+    landing:
+
+      # those two must apply simultaneously
+      landing_cutoff_mass_factor:  0.75 # how much lighter does the drone appear to be?
+      landing_cutoff_timeout:  0.5 # [s] how long does the throttle has to be below the mass factor
+
+    takeoff:
+
+      during_takeoff:
+        controller: "MpcController"
+
+      after_takeoff:
+        controller: "MpcController"
+
+  control_manager:
+
+    safety:
+
+      # emergency landing (still feedback)
+      eland:
+
+        cutoff_mass_factor:  0.75 # how much lighter does the drone appear to be?
+        cutoff_timeout:  0.5 # [s] how long does the throttle has to be below the mass factor

docker/shared_data/vio/shared_data/precise_landing/apriltag.yaml

@@ -0,0 +1,24 @@
+# AprilTag 3 code parameters
+# Find descriptions in apriltag/include/apriltag.h:struct apriltag_detector
+#                      apriltag/include/apriltag.h:struct apriltag_family
+tag_family:        'tagCustom48h12' # options: tagStandard52h13, tagStandard41h12, tag36h11, tag25h9, tag16h5, tagCustom48h12, tagCircle21h7, tagCircle49h12
+tag_threads:       2          # default: 2
+tag_decimate:      1.0        # default: 1.0
+tag_blur:          0.0        # default: 0.0
+tag_refine_edges:  1          # default: 1
+tag_debug:         0          # default: 0
+max_hamming_dist:  2          # default: 2 (Tunable parameter with 2 being a good choice - values >=3 consume large amounts of memory. Choose the largest value possible.)
+# Other parameters
+publish_tf:        true       # default: false
+transport_hint:    "raw"      # default: raw, see http://wiki.ros.org/image_transport#Known_Transport_Packages for options
+
+# standalone_tags:
+#   [
+#     {id: ID, size: SIZE, name: NAME},
+#     ...
+#   ]
+standalone_tags:
+  [
+    {id: 0, size: 0.129, name: recursive_tag_1_big},
+    {id: 10, size: 0.0255, name: recursive_tag_1_small},
+  ]

docker/shared_data/vio/shared_data/precise_landing/landing_controller.yaml

@@ -0,0 +1,26 @@
+# what controller will be used for the landing
+controller: "MpcController"
+
+desired_heading:
+
+  relative_to_pad:
+    enabled: false
+    heading: 0 # [rad]
+
+stages:
+
+  aligning:
+    speed: 0.2    # [m/s]
+    height: 1.3   # [m]
+
+  repeating:
+    height: 1.3 # [m]
+
+  aborting:
+    height: 1.3 # [m]
+
+  descending:
+    speed: 0.2    # [m/s]
+
+  landing:
+    speed: 0.2          # [m/s]

docker/shared_data/vio/shared_data/precise_landing/landing_estimator.yaml

@@ -0,0 +1,6 @@
+# the list of tag ids order according to their priority (highest-to-lowest)
+tag_ids: [0, 10] # [-]
+
+# the estimation will happen in this frame of refernece
+# (detections are transformed to this frame first)
+estimation_frame: "fixed_origin"

docker/shared_data/vio/shared_data/record.sh

@@ -19,6 +19,8 @@ exclude=(
 # don't record cameras
 '(.*)camera_(.*)'
 
+'(.*)apriltag(.*)'
+
 '(.*)ov_msckf(.*)'
 
 '(.*)mavros(.*)'

docker/shared_data/vio/shared_data/world_local.yaml

@@ -7,7 +7,7 @@ world_origin:
 
 safety_area:
 
-  enabled: true
+  enabled: false
 
   horizontal:
 
@@ -31,4 +31,4 @@ safety_area:
     frame_name: "fixed_origin"
 
     max_z: 3.0
-    min_z: 0.0
+    min_z: -1.0

docker/uav_sessions/gnss/compose.yaml

@@ -70,7 +70,7 @@ services:
       - shared_data:/etc/docker/shared_data:consistent
     env_file:
       - ./stack.env
-    command: bash -c "waitForHw && roslaunch mrs_uav_core core.launch platform_config:=`rospack find mrs_robofly_core`/config/platform_config.yaml custom_config:=/etc/docker/shared_data/custom_config.yaml world_config:=/etc/docker/shared_data/world_local.yaml network_config:=/etc/docker/shared_data/network_config.yaml"
+    command: bash -c "waitForHw && roslaunch mrs_uav_core core.launch platform_config:=`rospack find mrs_robofly_core`/config/platform_config.yaml custom_config:=/etc/docker/shared_data/custom_config.yaml world_config:=/etc/docker/shared_data/world_temesvar_field.yaml network_config:=/etc/docker/shared_data/network_config.yaml"
 
   # starts the camera drivers
   cameras:
@@ -100,6 +100,8 @@ services:
       - ./stack.env
     command: bash -c "waitForHw && roslaunch mrs_uav_autostart automatic_start.launch"
 
+
+
   # starts `rosbag record`
   rosbag:
     image: ctumrs/mrs_uav_system:${MRS_UAV_SYSTEM_VERSION}

docker/uav_sessions/precise_landing/compose.yaml

@@ -0,0 +1,54 @@
+volumes:
+
+  catkin_workspace:
+
+  shared_data:
+
+services:
+
+  # will copy session-specific data shared between containers from the shared_data container to a shared volume
+  copy_shared_data:
+    image: fly4future/robofly:shared_data_vio
+    volumes:
+      - shared_data:/tmp/docker/shared_data:consistent
+    command: sh -c "rm -rvf /tmp/docker/shared_data/*; mkdir -pv /tmp/docker/shared_data; cp -rv /etc/docker/shared_data/* /tmp/docker/shared_data/"
+
+  # will copy user's ROS catkin workspace from the 'transfer' alpine image to a shared volume
+  copy_catkin_workspace:
+    image: precise_landing:1.0.0
+    volumes:
+      - catkin_workspace:/tmp/docker/catkin_workspace:consistent
+    command: sh -c "rm -rvf /tmp/docker/catkin_workspace/*; mkdir -pv /tmp/docker/catkin_workspace; cp -rv /etc/docker/catkin_workspace/* /tmp/docker/catkin_workspace/"
+
+  # starts the HW API for connecting the MRS UAV System to PX4
+  custom_package:
+    image: ctumrs/mrs_uav_system:${MRS_UAV_SYSTEM_VERSION}
+    network_mode: host
+    depends_on:
+      - copy_catkin_workspace
+      - copy_shared_data
+    volumes:
+      - catkin_workspace:/etc/docker/catkin_workspace:consistent
+      - shared_data:/etc/docker/shared_data:consistent
+    env_file:
+      - ./stack.env
+    command: bash -c "sleep 1 && source /etc/docker/catkin_workspace/devel/setup.bash && waitForRos && roslaunch mrs_precise_landing precise_landing.launch apriltag_config:=/etc/docker/shared_data/precise_landing/apriltag.yaml camera_node:=camera_down_throttled image_topic:=image_raw estimator_config:=/etc/docker/shared_data/precise_landing//landing_estimator.yaml controller_config:=/etc/docker/shared_data/precise_landing//landing_controller.yaml"
+
+  # this container can be used to access a terminal with ROS inside the compose session
+  terminal:
+    image: ctumrs/mrs_uav_system:${MRS_UAV_SYSTEM_VERSION}
+    network_mode: host
+    depends_on:
+      - copy_catkin_workspace
+      - copy_shared_data
+    env_file:
+      - ./stack.env
+    entrypoint: ["/bin/bash", "-c"]
+    volumes:
+      - catkin_workspace:/etc/docker/catkin_workspace:consistent
+      - /dev/:/dev/
+    command:
+      - bash --rcfile /etc/docker/catkin_workspace/devel/setup.bash
+    privileged: true
+    stdin_open: true
+    tty: true

docker/uav_sessions/precise_landing/variables.env

@@ -0,0 +1,3 @@
+UAV_NAME=uav80
+ROS_MASTER_URI=http://localhost:11311
+MRS_UAV_SYSTEM_VERSION=1.5.0

docker/uav_sessions/vio/compose.yaml

@@ -6,6 +6,8 @@ volumes:
 
   bag_files:
 
+  catkin_workspace:
+
 services:
 
   # will copy session-specific data shared between containers from the shared_data container to a shared volume
@@ -22,6 +24,13 @@ services:
       - uav_custom_files:/tmp/docker/uav_custom_files:consistent
     command: sh -c "rm -rvf /tmp/docker/uav_custom_files/*; mkdir -pv /tmp/docker/uav_custom_files; cp -rv /etc/docker/uav_custom_files/${UAV_TYPE}/${UAV_ID}/* /tmp/docker/uav_custom_files/"
 
+  # will copy user's ROS catkin workspace from the 'transfer' alpine image to a shared volume
+  copy_catkin_workspace:
+    image: precise_landing:1.0.0
+    volumes:
+      - catkin_workspace:/tmp/docker/catkin_workspace:consistent
+    command: sh -c "rm -rvf /tmp/docker/catkin_workspace/*; mkdir -pv /tmp/docker/catkin_workspace; cp -rv /etc/docker/catkin_workspace/* /tmp/docker/catkin_workspace/"
+
   # starts roscore
   # this is the first container in the ROS pipeline
   roscore:
@@ -68,10 +77,12 @@ services:
     network_mode: host
     volumes:
       - shared_data:/etc/docker/shared_data:consistent
+      - uav_custom_files:/etc/docker/uav_custom_files:consistent
       - /dev/:/dev/
     env_file:
       - ./stack.env
-    command: bash -c "waitForHw && roslaunch mrs_robofly_core open_vins.launch verbosity:=INFO"
+    command: bash -c "waitForHw && roslaunch mrs_robofly_core open_vins.launch custom_config:=/etc/docker/uav_custom_files/open_vins/open_vins.yaml"
+
 
   # starts the MRS UAV System's core
   uav_core:
@@ -108,10 +119,11 @@ services:
       - rostime
     network_mode: host
     volumes:
-      - shared_data:/etc/docker/shared_data:consistent
+      - shared_data:/etc/docker/shared_data:consistent                
+      - catkin_workspace:/etc/docker/catkin_workspace:consistent 
     env_file:
       - ./stack.env
-    command: bash -c "waitForHw && roslaunch mrs_uav_autostart automatic_start.launch"
+    command: bash -c "source /etc/docker/catkin_workspace/devel/setup.bash && waitForHw && roslaunch mrs_uav_autostart automatic_start.launch"
 
   # starts `rosbag record`
   rosbag:
@@ -140,6 +152,7 @@ services:
       - shared_data:/etc/docker/shared_data:consistent
       - uav_custom_files:/etc/docker/uav_custom_files:consistent
       - /dev/:/dev/
+      - catkin_workspace:/etc/docker/catkin_workspace:consistent
     command:
       - bash --rcfile /opt/ros/noetic/setup.bash
     privileged: true

docker/user_workspace/Dockerfile

@@ -38,4 +38,4 @@ COPY ./cache/${WORKSPACE_PATH}/src /${WORKSPACE_PATH}/src
 COPY ./cache/${WORKSPACE_PATH}/.catkin_tools /${WORKSPACE_PATH}/.catkin_tools
 COPY ./cache/${WORKSPACE_PATH}/devel /${WORKSPACE_PATH}/devel
 
-CMD ["bash"]
+CMD ["sh"]

docker/user_workspace/build_image.sh

@@ -15,7 +15,7 @@ cd ${MY_PATH}
 ## |                            setup                           |
 ## --------------------------------------------------------------
 
-LOCAL_TAG=user_workspace:1.0.0
+LOCAL_TAG=precise_landing:1.0.0
 
 ARCH=arm64 # robofly
 # ARCH=amd64

docker/user_workspace/export_image.sh

@@ -15,7 +15,7 @@ cd ${MY_PATH}
 ## |                            setup                           |
 ## --------------------------------------------------------------
 
-LOCAL_TAG=user_workspace:1.0.0
+LOCAL_TAG=precise_landing:1.0.0
 EXPORT_PATH=~/docker
 
 ## --------------------------------------------------------------