simple_offboard: correctly check manual control timeout, separate it from kill switch check

aruco_pose/CMakeLists.txt

@@ -180,13 +180,6 @@ target_link_libraries(aruco_pose
   ${OpenCV_LIBRARIES}
 )
 
-# Prevent aruco_pose from having undefined symbols
-set_property(TARGET aruco_pose
-  APPEND
-  PROPERTY LINK_FLAGS
-  -Wl,--no-undefined
-)
-
 #############
 ## Install ##
 #############

aruco_pose/src/genmap.py

@@ -13,7 +13,7 @@
 Generate map file for aruco_map nodelet.
 
 Usage:
-  genmap.py <length> <x> <y> <dist_x> <dist_y> [<first>] [<x0>] [<y0>] [--top-left | --bottom-left]
+  genmap.py <length> <x> <y> <dist_x> <dist_y> [<first>] [<x0>] [<y0>] [--top-left | --bottom-left] [-o <filename>]
   genmap.py (-h | --help)
 
 Options:
@@ -27,6 +27,7 @@ Options:
   <y0>           Y coordinate for the first marker [default: 0]
   --top-left     First marker is on top-left (default)
   --bottom-left  First marker is on bottom-left
+  -o <filename>  Output map file name in the 'map' subdirectory of aruco_pose package
 
 Example:
   rosrun aruco_pose genmap.py 0.33 2 4 1 1 0 > $(catkin_find aruco_pose map)/test_map.txt
@@ -34,6 +35,8 @@ Example:
 
 from __future__ import print_function
 
+import sys
+from os import path
 from docopt import docopt
 
 
@@ -49,14 +52,19 @@ dist_x = float(arguments['<dist_x>'])
 dist_y = float(arguments['<dist_y>'])
 bottom_left = arguments['--bottom-left']
 
+if arguments['-o'] is None:
+    output = sys.stdout
+else:
+    output = open(path.join(path.dirname(__file__), '..', 'map', arguments['-o']), 'w')
+
 max_y = y0 + (markers_y - 1) * dist_y
 
-print('# id\tlength\tx\ty\tz\trot_z\trot_y\trot_x')
+output.write('# id\tlength\tx\ty\tz\trot_z\trot_y\trot_x\n')
 for y in range(markers_y):
     for x in range(markers_x):
         pos_x = x0 + x * dist_x
         pos_y = y0 + y * dist_y
         if not bottom_left:
             pos_y = max_y - pos_y
-        print('{}\t{}\t{}\t{}\t{}\t{}\t{}\t{}'.format(first, length, pos_x, pos_y, 0, 0, 0, 0))
+        output.write('{}\t{}\t{}\t{}\t{}\t{}\t{}\t{}\n'.format(first, length, pos_x, pos_y, 0, 0, 0, 0))
         first += 1

aruco_pose/vendor/VendorOpenCV.cmake

@@ -7,7 +7,6 @@ endif()
 
 message(STATUS "Adding vendored aruco_pose OpenCV module")
 add_library(_opencv_aruco STATIC
-  vendor/aruco/src/apriltag_quad_thresh.cpp
   vendor/aruco/src/aruco.cpp
   vendor/aruco/src/charuco.cpp
   vendor/aruco/src/dictionary.cpp

aruco_pose/vendor/aruco/src/apriltag_quad_thresh.cpp

@@ -94,7 +94,7 @@ void ptsort_(struct pt *pts, int sz)
 
     // Use stack storage if it's not too big.
     cv::AutoBuffer<struct pt, 1024> _tmp_stack(sz);
-    memcpy(_tmp_stack, pts, sizeof(struct pt) * sz);
+    memcpy(_tmp_stack.data(), pts, sizeof(struct pt) * sz);
 
     int asz = sz/2;
     int bsz = sz - asz;
@@ -470,11 +470,11 @@ int quad_segment_agg(int sz, struct line_fit_pt *lfps, int indices[4]){
     int rvalloc_pos = 0;
     int rvalloc_size = 3*sz;
     cv::AutoBuffer<struct remove_vertex, 0> rvalloc_(std::max(1, rvalloc_size));
-    memset(rvalloc_, 0, sizeof(rvalloc_[0]) * rvalloc_.size()); // TODO Add AutoBuffer zero fill
-    struct remove_vertex *rvalloc = rvalloc_;
+    memset(rvalloc_.data(), 0, sizeof(rvalloc_[0]) * rvalloc_.size()); // TODO Add AutoBuffer zero fill
+    struct remove_vertex *rvalloc = rvalloc_.data();
     cv::AutoBuffer<struct segment, 0> segs_(std::max(1, sz)); // TODO Add AutoBuffer zero fill
-    memset(segs_, 0, sizeof(segs_[0]) * segs_.size());
-    struct segment *segs = segs_;
+    memset(segs_.data(), 0, sizeof(segs_[0]) * segs_.size());
+    struct segment *segs = segs_.data();
 
     // populate with initial entries
     for (int i = 0; i < sz; i++) {
@@ -753,8 +753,8 @@ int fit_quad(const Ptr<DetectorParameters> &_params, const Mat im, zarray_t *clu
     // efficiently computed for any contiguous range of indices.
 
     cv::AutoBuffer<struct line_fit_pt, 64> lfps_(sz);
-    memset(lfps_, 0, sizeof(lfps_[0]) * lfps_.size()); // TODO Add AutoBuffer zero fill
-    struct line_fit_pt *lfps = lfps_;
+    memset(lfps_.data(), 0, sizeof(lfps_[0]) * lfps_.size()); // TODO Add AutoBuffer zero fill
+    struct line_fit_pt *lfps = lfps_.data();
 
     for (int i = 0; i < sz; i++) {
         struct pt *p;

clover/launch/aruco.launch

@@ -2,6 +2,9 @@
     <arg name="aruco_detect" default="true"/>
     <arg name="aruco_map" default="false"/>
     <arg name="aruco_vpe" default="false"/>
+    <arg name="placement" default="floor"/> <!-- markers placement: floor, ceiling, unknown -->
+    <arg name="length" default="0.33"/> <!-- not-in-map markers length, m -->
+    <arg name="map" default="map.txt"/> <!-- markers map file name -->
 
     <!-- For additional help go to https://clover.coex.tech/aruco -->
 
@@ -12,11 +15,14 @@
         <remap from="map_markers" to="aruco_map/markers" if="$(arg aruco_map)"/>
         <param name="estimate_poses" value="true"/>
         <param name="send_tf" value="true"/>
-        <param name="known_tilt" value="map"/>
-        <param name="length" value="0.33"/>
+        <param name="known_tilt" value="map" if="$(eval placement == 'floor')"/>
+        <param name="known_tilt" value="map_flipped" if="$(eval position == 'ceiling')"/>
+        <param name="length" value="$(arg length)"/>
         <!-- aruco detector parameters -->
         <param name="cornerRefinementMethod" value="2"/> <!-- contour refinement -->
         <param name="minMarkerPerimeterRate" value="0.075"/> <!-- 0.075 for 320x240, 0.0375 for 640x480 -->
+        <!-- length override example: -->
+        <!-- <param name="length_override/3" value="0.1"/> -->
     </node>
 
     <!-- aruco_map: estimate aruco map pose -->
@@ -24,8 +30,9 @@
         <remap from="image_raw" to="main_camera/image_raw"/>
         <remap from="camera_info" to="main_camera/camera_info"/>
         <remap from="markers" to="aruco_detect/markers"/>
-        <param name="map" value="$(find aruco_pose)/map/map.txt"/>
-        <param name="known_tilt" value="map"/>
+        <param name="map" value="$(find aruco_pose)/map/$(arg map)"/>
+        <param name="known_tilt" value="map" if="$(eval placement == 'floor')"/>
+        <param name="known_tilt" value="map_flipped" if="$(eval placement == 'ceiling')"/>
         <param name="image_axis" value="true"/>
         <param name="frame_id" value="aruco_map_detected" if="$(arg aruco_vpe)"/>
         <param name="frame_id" value="aruco_map" unless="$(arg aruco_vpe)"/>

clover/src/simple_offboard.cpp

@@ -73,6 +73,7 @@ ros::Duration state_timeout;
 ros::Duration velocity_timeout;
 ros::Duration global_position_timeout;
 ros::Duration battery_timeout;
+ros::Duration manual_control_timeout;
 float default_speed;
 bool auto_release;
 bool land_only_in_offboard, nav_from_sp, check_kill_switch;
@@ -488,16 +489,19 @@ void publishSetpoint(const ros::TimerEvent& event)
 	publish(event.current_real);
 }
 
-inline void checkKillSwitch()
+inline void checkManualControl()
 {
-	if (!TIMEOUT(manual_control, state_timeout))
-		throw std::runtime_error("Manual control timeout, can't check kill switch status");
+	if (!manual_control_timeout.isZero() && TIMEOUT(manual_control, manual_control_timeout)) {
+		throw std::runtime_error("Manual control timeout, RC is switched off?");
+	}
 
-	const int KILL_SWITCH_BIT = 12; // https://github.com/PX4/Firmware/blob/c302514a0809b1765fafd13c014d705446ae1113/src/modules/mavlink/mavlink_messages.cpp#L3975
-	bool kill_switch = manual_control.buttons & (1 << KILL_SWITCH_BIT);
+	if (check_kill_switch) {
+		const int KILL_SWITCH_BIT = 12; // https://github.com/PX4/Firmware/blob/c302514a0809b1765fafd13c014d705446ae1113/src/modules/mavlink/mavlink_messages.cpp#L3975
+		bool kill_switch = manual_control.buttons & (1 << KILL_SWITCH_BIT);
 
-	if (kill_switch)
-		throw std::runtime_error("Kill switch is on");
+		if (kill_switch)
+			throw std::runtime_error("Kill switch is on");
+	}
 }
 
 inline void checkState()
@@ -527,8 +531,8 @@ bool serve(enum setpoint_type_t sp_type, float x, float y, float z, float vx, fl
 		// Checks
 		checkState();
 
-		if (auto_arm && check_kill_switch) {
-			checkKillSwitch();
+		if (auto_arm) {
+			checkManualControl();
 		}
 
 		// default frame is local frame
@@ -862,6 +866,7 @@ int main(int argc, char **argv)
 	velocity_timeout = ros::Duration(nh_priv.param("velocity_timeout", 2.0));
 	global_position_timeout = ros::Duration(nh_priv.param("global_position_timeout", 10.0));
 	battery_timeout = ros::Duration(nh_priv.param("battery_timeout", 2.0));
+	manual_control_timeout = ros::Duration(nh_priv.param("manual_control_timeout", 0.0));
 
 	transform_timeout = ros::Duration(nh_priv.param("transform_timeout", 0.5));
 	telemetry_transform_timeout = ros::Duration(nh_priv.param("telemetry_transform_timeout", 0.5));

docs/en/aruco_map.md

@@ -1,5 +1,9 @@
 # Map-based navigation with ArUco markers
 
+> **Note** The following applies to [image versions](image.md) **0.22** and up. Older documentation is still available for [for version **0.20**](https://github.com/CopterExpress/clover/blob/v0.20/docs/en/aruco_map.md).
+
+<!-- -->
+
 > **Info** Marker detection requires the camera module to be correctly plugged in and [configured](camera_setup.md).
 
 <!-- -->
@@ -39,18 +43,19 @@ marker_id marker_size x y z z_angle y_angle x_angle
 
 `N_angle` is the angle of rotation along the `N` axis in radians.
 
-Map path is defined in the `map` parameter:
+Файлы карт располагаются в каталоге `~/catkin_ws/src/clover/aruco_pose/map`. Название файла с картой задается в аргументе `map`:
+Map files are located at the `~/catkin_ws/src/clover/aruco_pose/map` directory. Map file name is defined in the `map` argument:
 
 ```xml
-<param name="map" value="$(find aruco_pose)/map/map.txt"/>
+<arg name="map" default="map.txt"/>
 ```
 
-Some map examples are provided in [`~/catkin_ws/src/clover/aruco_pose/map`](https://github.com/CopterExpress/clover/tree/master/aruco_pose/map).
+Some map examples are provided in [the directory](https://github.com/CopterExpress/clover/tree/master/aruco_pose/map).
 
 Grid maps may be generated using the `genmap.py` script:
 
 ```bash
-rosrun aruco_pose genmap.py length x y dist_x dist_y first > ~/catkin_ws/src/clover/aruco_pose/map/test_map.txt
+rosrun aruco_pose genmap.py length x y dist_x dist_y first -o test_map.txt
 ```
 
 `length` is the size of each marker, `x` is the marker count along the *x* axis, `y` is the marker count along the *y* axis, `dist_x` is the distance between the centers of adjacent markers along the *x* axis, `dist_y` is the distance between the centers of the *y* axis, `first` is the ID of the first marker (top left marker, unless `--bottom-left` is specified), `test_map.txt` is the name of the generated map file. The optional `--bottom-left` parameter changes the numbering of markers, making the bottom left marker the first one.
@@ -58,7 +63,7 @@ rosrun aruco_pose genmap.py length x y dist_x dist_y first > ~/catkin_ws/src/clo
 Usage example:
 
 ```bash
-rosrun aruco_pose genmap.py 0.33 2 4 1 1 0 > ~/catkin_ws/src/clover/aruco_pose/map/test_map.txt
+rosrun aruco_pose genmap.py 0.33 2 4 1 1 0 -o test_map.txt
 ```
 
 Additional information on the utility can be obtained using `-h` key: `rosrun aruco_pose genmap.py -h`.
@@ -152,10 +157,10 @@ If the drone's altitude is not stable, try increasing the `MPC_Z_VEL_P` paramete
 
 In order to navigate using markers on the ceiling, mount the onboard camera so that it points up and [adjust the camera frame accordingly](camera_setup.md).
 
-You should also set the `known_tilt` parameter to `map_flipped` in both `aruco_detect` and `aruco_map` sections of `~/catkin_ws/src/clover/clover/launch/aruco.launch`:
+You should also set the `placement` parameter to `ceilin` in `~/catkin_ws/src/clover/clover/launch/aruco.launch`:
 
 ```xml
-<param name="known_tilt" value="map_flipped"/>
+<arg name="placement" default="ceiling"/>
 ```
 
 This will flip the `aruco_map` frame (making its **<font color=blue>z</font>** axis point downward). Thus, in order to fly 2 metres below ceiling, the `z` argument for the `navigate` service should be set to 2:

docs/en/aruco_marker.md

@@ -1,5 +1,9 @@
 # ArUco marker detection
 
+> **Note** The following applies to [image versions](image.md) **0.22** and up. Older documentation is still available for [for version **0.20**](https://github.com/CopterExpress/clover/blob/v0.20/docs/en/aruco_marker.md).
+
+<!-- -->
+
 > **Info** Marker detection requires the camera module to be correctly plugged in and [configured](camera.md).
 
 `aruco_detect` module detects ArUco markers and publishes their positions in ROS topics and as [TF frames](frames.md).
@@ -22,22 +26,20 @@ For enabling detection set the `aruco_detect` argument in `~/catkin_ws/src/clove
 <arg name="aruco_detect" default="true"/>
 ```
 
-For the module to work correctly the following parameters should be set:
+For the module to work correctly the following arguments should also be set:
 
 ```xml
-<param name="length" value="0.32"/>          <!-- length of a single marker, in meters (excluding the white border) -->
-<param name="estimate_poses" value="true"/>  <!-- position estimation for single markers -->
-<param name="send_tf" value="true"/>         <!-- TF frame creation for markers -->
-<param name="known_tilt" value="map"/>       <!-- Marker tilt, explained below -->
+<arg name="placement" default="floor"/> <!-- markers' placement, explained below  -->
+<arg name="length" default="0.33"/>     <!-- length of a single marker, in meters (excluding the white border) -->
 ```
 
-`known_tilt` should be set to:
+`placement` argument should be set to:
 
-* `map` if *all* markers are on the ground;
-* `map_flipped` if *all* markers are on the ceiling;
+* `floor` if *all* markers are on the ground;
+* `ceiling` if *all* markers are on the ceiling;
 * an empty string otherwise.
 
-You may specify length for each marker individually by using the `length_override` parameter:
+You may specify length for each marker individually by using the `length_override` parameter of the node `aruco_detect`:
 
 ```xml
 <param name="length_override/3" value="0.1"/>    <!-- marker with id=3 has a side of 0.1m -->

docs/ru/aruco_map.md

@@ -1,5 +1,9 @@
 # Навигация по картам ArUco-маркеров
 
+> **Note** Документация для версий [образа](image.md), начиная с версии **0.22**. Для более ранних версий см. [документацию для версии **0.20**](https://github.com/CopterExpress/clover/blob/v0.20/docs/ru/aruco_map.md).
+
+<!-- -->
+
 > **Info** Для распознавания маркеров модуль камеры должен быть корректно подключен и [сконфигурирован](camera.md).
 
 <!-- -->
@@ -39,18 +43,18 @@ id_маркера размер_маркера x y z угол_z угол_y уго
 
 Где `угол_N` – это угол поворота маркера вокруг оси N в радианах.
 
-Путь к файлу с картой задается в параметре `map`:
+Файлы карт располагаются в каталоге `~/catkin_ws/src/clover/aruco_pose/map`. Название файла с картой задается в аргументе `map`:
 
 ```xml
-<param name="map" value="$(find aruco_pose)/map/map.txt"/>
+<arg name="map" default="map.txt"/>
 ```
 
-Смотрите примеры карт маркеров в каталоге [`~/catkin_ws/src/clover/aruco_pose/map`](https://github.com/CopterExpress/clover/tree/master/aruco_pose/map).
+Смотрите примеры карт маркеров в [`вышеуказанном каталоге`](https://github.com/CopterExpress/clover/tree/master/aruco_pose/map).
 
 Файл карты может быть сгенерирован с помощью инструмента `genmap.py`:
 
 ```bash
-rosrun aruco_pose genmap.py length x y dist_x dist_y first > ~/catkin_ws/src/clover/aruco_pose/map/test_map.txt
+rosrun aruco_pose genmap.py length x y dist_x dist_y first -o test_map.txt
 ```
 
 Где `length` – размер маркера, `x` – количество маркеров по оси *x*, `y` - количество маркеров по оси *y*, `dist_x` – расстояние между центрами маркеров по оси *x*, `y` – расстояние между центрами маркеров по оси *y*, `first` – ID первого (левого нижнего) маркера, `test_map.txt` – название файла с картой. Дополнительный ключ `--bottom-left` позволяет нумеровать маркеры с левого нижнего угла.
@@ -58,7 +62,7 @@ rosrun aruco_pose genmap.py length x y dist_x dist_y first > ~/catkin_ws/src/clo
 Пример:
 
 ```bash
-rosrun aruco_pose genmap.py 0.33 2 4 1 1 0 > ~/catkin_ws/src/clover/aruco_pose/map/test_map.txt
+rosrun aruco_pose genmap.py 0.33 2 4 1 1 0 -o test_map.txt
 ```
 
 Дополнительную информацию по утилите можно получить по ключу `-h`: `rosrun aruco_pose genmap.py -h`.
@@ -154,10 +158,10 @@ navigate(frame_id='aruco_5', x=0, y=0, z=1)
 
 Для навигации по маркерам, расположенным на потолке, необходимо поставить основную камеру так, чтобы она смотрела вверх и [установить соответствующий фрейм камеры](camera_setup.md#frame).
 
-Также в файле `~/catkin_ws/src/clover/clover/launch/aruco.launch` необходимо установить параметр `known_tilt` в секциях `aruco_detect` и `aruco_map` в значение `map_flipped`:
+Также в файле `~/catkin_ws/src/clover/clover/launch/aruco.launch` необходимо выставить аргумент `placement` в значение `ceiling`:
 
 ```xml
-<param name="known_tilt" value="map_flipped"/>
+<arg name="placement" default="ceiling"/>
 ```
 
 При такой конфигурации фрейм `aruco_map` также окажется перевернутым. Таким образом, для полета на высоту 2 метра ниже потолка, аргумент `z` нужно устанавливать в 2:

docs/ru/aruco_marker.md

@@ -1,5 +1,9 @@
 # Распознавание ArUco-маркеров
 
+> **Note** Документация для версий [образа](image.md), начиная с версии **0.22**. Для более ранних версий см. [документацию для версии **0.20**](https://github.com/CopterExpress/clover/blob/v0.20/docs/ru/aruco_marker.md).
+
+<!-- -->
+
 > **Info** Для распознавания маркеров модуль камеры должен быть корректно подключен и [сконфигурирован](camera_setup.md).
 
 Модуль `aruco_detect` распознает ArUco-маркеры и публикует их позиции в ROS-топики и в [TF](frames.md).
@@ -22,22 +26,20 @@
 <arg name="aruco_detect" default="true"/>
 ```
 
-Для правильной работы в этом же файле в секции `aruco_detect` должны быть выставлены параметры:
+Для правильной работы в этом же файле также должны быть выставлены аргументы:
 
 ```xml
-<param name="length" value="0.32"/>          <!-- размер маркеров в метрах (не включая белую рамку) -->
-<param name="estimate_poses" value="true"/>  <!-- включение вычисления позиций маркеров -->
-<param name="send_tf" value="true"/>         <!-- отправлять позиции маркеров в виде TF-фреймов -->
-<param name="known_tilt" value="map"/>       <!-- наклон маркеров, см. далее -->
+<arg name="placement" default="floor"/> <!-- расположение маркеров, см. далее -->
+<arg name="length" default="0.33"/>     <!-- размер маркеров в метрах (не включая белую рамку) -->
 ```
 
-Значение параметра `known_tilt` следует выставлять следующим образом:
+Значение аргумента `placement` следует выставлять следующим образом:
 
-* если *все* маркеры наклеены на полу (земле), выставить значение `map`;
-* если *все* маркеры наклеены на потолке, выставить значение `map_flipped`;
+* если *все* маркеры наклеены на полу (земле), выставить значение `floor`;
+* если *все* маркеры наклеены на потолке, выставить значение `ceiling`;
 * в противном случае удалить строку с параметром.
 
-Если некоторые маркеры имеют размер, отличный значения `length`, их размер может быть переопределен с помощью параметра `length_override`:
+Если некоторые маркеры имеют размер, отличный значения `length`, их размер может быть переопределен с помощью параметра `length_override` ноды `aruco_detect`:
 
 ```xml
 <param name="length_override/3" value="0.1"/>    <!-- маркер c id 3 имеет размер 10 см -->