Remove marker 17 as of too common faults + remove CRLF

.travis.yml

@@ -9,7 +9,7 @@ env:
     - if [[ -z ${TRAVIS_TAG} ]]; then IMAGE_VERSION="${TRAVIS_COMMIT}}"; else IMAGE_VERSION="${TRAVIS_TAG}"; fi
     - IMAGE_NAME="$(basename -s '.git' ${TARGET_REPO})_${IMAGE_VERSION}.img"
 git:
-  depth: 1
+  depth: 50
 matrix:
   fast_finish: true
   include:
@@ -39,6 +39,7 @@ matrix:
         on:
           tags: true
         draft: true
+        name: ${TRAVIS_TAG}
     - name: "Documentation"
       language: node_js
       node_js:

apps/ios/cleverrc/Assets.xcassets/AppIcon.appiconset/Contents.json

@@ -145,8 +145,15 @@
       "size" : "44x44",
       "idiom" : "watch",
       "scale" : "2x",
-      "role" : "longLook",
-      "subtype" : "42mm"
+      "role" : "appLauncher",
+      "subtype" : "40mm"
+    },
+    {
+      "size" : "50x50",
+      "idiom" : "watch",
+      "scale" : "2x",
+      "role" : "appLauncher",
+      "subtype" : "44mm"
     },
     {
       "size" : "86x86",
@@ -162,10 +169,24 @@
       "role" : "quickLook",
       "subtype" : "42mm"
     },
+    {
+      "size" : "108x108",
+      "idiom" : "watch",
+      "scale" : "2x",
+      "role" : "quickLook",
+      "subtype" : "44mm"
+    },
     {
       "idiom" : "watch-marketing",
       "size" : "1024x1024",
       "scale" : "1x"
+    },
+    {
+      "size" : "44x44",
+      "idiom" : "watch",
+      "scale" : "2x",
+      "role" : "longLook",
+      "subtype" : "42mm"
     }
   ],
   "info" : {

apps/ios/cleverrc/Info.plist

@@ -17,9 +17,9 @@
 	<key>CFBundlePackageType</key>
 	<string>APPL</string>
 	<key>CFBundleShortVersionString</key>
-	<string>1.1</string>
+	<string>1.2</string>
 	<key>CFBundleVersion</key>
-	<string>6</string>
+	<string>7</string>
 	<key>LSRequiresIPhoneOS</key>
 	<true/>
 	<key>UILaunchStoryboardName</key>

aruco_pose/CMakeLists.txt

@@ -212,3 +212,8 @@ target_link_libraries(aruco_pose
 
 ## Add folders to be run by python nosetests
 # catkin_add_nosetests(test)
+
+if (CATKIN_ENABLE_TESTING)
+  find_package(rostest REQUIRED)
+  add_rostest(test/basic.test)
+endif()

aruco_pose/map/office_ceiling.txt

@@ -0,0 +1,31 @@
+14  0.365 0.000  0.0 0 0 0 0
+15  0.365 1.335  0.0 0 0 0 0
+30  0.365 2.865  0.0 0 0 0 0
+31  0.365 4.200  0.0 0 0 0 0
+12  0.365 0.000  1.8 0 0 0 0
+13	0.365	1.335	1.8	0	0	0	0
+28	0.365	2.865	1.8	0	0	0	0
+29	0.365	4.200	1.8	0	0	0	0
+10	0.365	0.000	3.6	0	0	0	0
+11	0.365	1.335	3.6	0	0	0	0
+26	0.365	2.865	3.6	0	0	0	0
+27	0.365	4.200	3.6	0	0	0	0
+8	0.365	0.000	5.4	0	0	0	0
+9	0.365	1.335	5.4	0	0	0	0
+24	0.365	2.865	5.4	0	0	0	0
+25	0.365	4.200	5.4	0	0	0	0
+6	0.365	0.000	7.2	0	0	0	0
+7	0.365	1.335	7.2	0	0	0	0
+22	0.365	2.865	7.2	0	0	0	0
+23	0.365	4.200	7.2	0	0	0	0
+4	0.365	0.000	9.0	0	0	0	0
+5	0.365	1.335	9.0	0	0	0	0
+20	0.365	2.865	9.0	0	0	0	0
+21	0.365	4.200	9.0	0	0	0	0
+2	0.365	0.000	0.8	0	0	0	0
+3	0.365	1.335	0.8	0	0	0	0
+18	0.365	2.865	0.8	0	0	0	0
+19	0.365	4.200	0.8	0	0	0	0
+1	0.365	0.000	2.6	0	0	0	0
+0	0.365	1.335	2.6	0	0	0	0
+16	0.365	2.865	2.6	0	0	0	0

aruco_pose/package.xml

@@ -29,6 +29,8 @@
   <depend>sensor_msgs</depend>
   <depend>rostest</depend>
 
+  <test_depend>image_publisher</test_depend>
+
   <!-- The export tag contains other, unspecified, tags -->
   <export>
     <nodelet plugin="${prefix}/nodelet_plugins.xml" />

aruco_pose/src/aruco_map.cpp

@@ -228,7 +228,6 @@ publish_debug:
 		}
 	}
 
-	// TODO consider z
 	void alignObjPointsToCenter(Mat &obj_points, double &center_x, double &center_y, double &center_z) const
 	{
 		// Align object points to the center of mass

aruco_pose/src/draw.cpp

@@ -74,24 +74,14 @@ void _drawPlanarBoard(Board *_board, Size outSize, OutputArray _img, int marginS
 
 		dictionary.drawMarker(_board->ids[m], side, marker, borderBits);
 
-		try {
-			if((outCorners[0].y == outCorners[1].y) && (outCorners[1].x == outCorners[2].x)) {
-				// marker is aligned to image axes
-				marker.copyTo(out(Rect(outCorners[0], dst_sz)));
-				continue;
-			}
+		// interpolate tiny marker to marker position in markerZone
+		inCorners[0] = Point2f(-0.5f, -0.5f);
+		inCorners[1] = Point2f(marker.cols - 0.5f, -0.5f);
+		inCorners[2] = Point2f(marker.cols - 0.5f, marker.rows - 0.5f);
 
-			// interpolate tiny marker to marker position in markerZone
-			inCorners[0] = Point2f(-0.5f, -0.5f);
-			inCorners[1] = Point2f(marker.cols - 0.5f, -0.5f);
-			inCorners[2] = Point2f(marker.cols - 0.5f, marker.rows - 0.5f);
-
-			// remove perspective
-			Mat transformation = getAffineTransform(inCorners, outCorners);
-			warpAffine(marker, out, transformation, out.size(), INTER_LINEAR,
-							BORDER_TRANSPARENT);
-		} catch(cv::Exception& e) {
-			ROS_INFO("Skip drawing marker %d", m);
-		}
+		// remove perspective
+		Mat transformation = getAffineTransform(inCorners, outCorners);
+		warpAffine(marker, out, transformation, out.size(), INTER_LINEAR,
+						BORDER_TRANSPARENT);
 	}
 }

aruco_pose/src/genmap.py

@@ -0,0 +1,47 @@
+#!/usr/bin/env python
+
+"""Markers map generator
+
+Generate map file for aruco_map nodelet.
+
+Usage:
+  genmap.py <length> <x> <y> <dist_x> <dist_y> <first> [--top-left]
+  genmap.py (-h | --help)
+
+Options:
+  <length>    Marker side length
+  <x>         Marker count along X axis
+  <y>         Marker count along Y axis
+  <dist_x>    Distance between markers along X axis
+  <dist_y>    Distance between markers along Y axis
+  <sep_x>     Space beetween markers along X axis
+  <sep_y>     Space beetween markers along Y axis
+  <first>     First marker ID
+  --top-left  First marker is on top-left (not bottom-left)
+"""
+
+from __future__ import print_function
+
+from docopt import docopt
+
+
+arguments = docopt(__doc__)
+
+length = float(arguments['<length>'])
+first = int(arguments['<first>'])
+markers_x = int(arguments['<x>'])
+markers_y = int(arguments['<y>'])
+dist_x = float(arguments['<dist_x>'])
+dist_y = float(arguments['<dist_y>'])
+top_left = arguments['--top-left']
+
+max_y = markers_y * length
+
+for y in range(markers_y):
+    for x in range(markers_x):
+        pos_x = x * dist_x
+        pos_y = y * dist_y
+        if top_left:
+            pos_y = max_y - pos_y
+        print('{}\t{}\t{}\t{}\t{}\t{}\t{}\t{}\t'.format(first, length, pos_x, pos_y, 0, 0, 0, 0))
+        first += 1

aruco_pose/test/basic.py

@@ -2,7 +2,6 @@
 import sys
 import unittest
 import json
-from pytest import approx
 import rospy
 import rostest
 
@@ -18,76 +17,76 @@ class TestArucoPose(unittest.TestCase):
 
     def test_markers(self):
         markers = rospy.wait_for_message('aruco_detect/markers', MarkerArray, timeout=5)
-        assert len(markers.markers) == 4
-        assert markers.header.frame_id == 'main_camera_optical'
+        self.assertEqual(len(markers.markers), 4)
+        self.assertEqual(markers.header.frame_id, 'main_camera_optical')
 
-        assert markers.markers[0].id == 2
-        assert markers.markers[0].length == approx(0.33)
-        assert markers.markers[0].pose.position.x == approx(0.36706567854)
-        assert markers.markers[0].pose.position.y == approx(0.290484516644)
-        assert markers.markers[0].pose.position.z == approx(2.18787602301)
-        assert markers.markers[0].pose.orientation.x == approx(0.993997406299)
-        assert markers.markers[0].pose.orientation.y == approx(-0.00532003481626)
-        assert markers.markers[0].pose.orientation.z == approx(-0.107390951553)
-        assert markers.markers[0].pose.orientation.w == approx(0.0201999263402)
-        assert markers.markers[0].c1.x == approx(415.557739258)
-        assert markers.markers[0].c1.y == approx(335.557739258)
-        assert markers.markers[0].c2.x == approx(509.442260742)
-        assert markers.markers[0].c2.y == approx(335.557739258)
-        assert markers.markers[0].c3.x == approx(509.442260742)
-        assert markers.markers[0].c3.y == approx(429.442260742)
-        assert markers.markers[0].c4.x == approx(415.557739258)
-        assert markers.markers[0].c4.y == approx(429.442260742)
+        self.assertEqual(markers.markers[0].id, 2)
+        self.assertAlmostEqual(markers.markers[0].length, 0.33, places=4)
+        self.assertAlmostEqual(markers.markers[0].pose.position.x, 0.36706567854, places=4)
+        self.assertAlmostEqual(markers.markers[0].pose.position.y, 0.290484516644, places=4)
+        self.assertAlmostEqual(markers.markers[0].pose.position.z, 2.18787602301, places=4)
+        self.assertAlmostEqual(markers.markers[0].pose.orientation.x, 0.993997406299, places=4)
+        self.assertAlmostEqual(markers.markers[0].pose.orientation.y, -0.00532003481626, places=4)
+        self.assertAlmostEqual(markers.markers[0].pose.orientation.z, -0.107390951553, places=4)
+        self.assertAlmostEqual(markers.markers[0].pose.orientation.w, 0.0201999263402, places=4)
+        self.assertAlmostEqual(markers.markers[0].c1.x, 415.557739258, places=4)
+        self.assertAlmostEqual(markers.markers[0].c1.y, 335.557739258, places=4)
+        self.assertAlmostEqual(markers.markers[0].c2.x, 509.442260742, places=4)
+        self.assertAlmostEqual(markers.markers[0].c2.y, 335.557739258, places=4)
+        self.assertAlmostEqual(markers.markers[0].c3.x, 509.442260742, places=4)
+        self.assertAlmostEqual(markers.markers[0].c3.y, 429.442260742, places=4)
+        self.assertAlmostEqual(markers.markers[0].c4.x, 415.557739258, places=4)
+        self.assertAlmostEqual(markers.markers[0].c4.y, 429.442260742, places=4)
 
-        assert markers.markers[3].id == 3
-        assert markers.markers[3].length == approx(0.1)
-        assert markers.markers[3].pose.position.x == approx(-0.1805169666)
-        assert markers.markers[3].pose.position.y == approx(-0.200697302327)
-        assert markers.markers[3].pose.position.z == approx(0.585767514823)
-        assert markers.markers[3].pose.orientation.x == approx(-0.961738074009)
-        assert markers.markers[3].pose.orientation.y == approx(-0.0375180244707)
-        assert markers.markers[3].pose.orientation.z == approx(-0.0115387773672)
-        assert markers.markers[3].pose.orientation.w == approx(0.271144115664)
-        assert markers.markers[3].c1.x == approx(129.557723999)
-        assert markers.markers[3].c1.y == approx(49.557723999)
-        assert markers.markers[3].c2.x == approx(223.442276001)
-        assert markers.markers[3].c2.y == approx(49.557723999)
-        assert markers.markers[3].c3.x == approx(223.442276001)
-        assert markers.markers[3].c3.y == approx(143.442276001)
-        assert markers.markers[3].c4.x == approx(129.557723999)
-        assert markers.markers[3].c4.y == approx(143.442276001)
+        self.assertEqual(markers.markers[3].id, 3)
+        self.assertAlmostEqual(markers.markers[3].length, 0.1, places=4)
+        self.assertAlmostEqual(markers.markers[3].pose.position.x, -0.1805169666, places=4)
+        self.assertAlmostEqual(markers.markers[3].pose.position.y, -0.200697302327, places=4)
+        self.assertAlmostEqual(markers.markers[3].pose.position.z, 0.585767514823, places=4)
+        self.assertAlmostEqual(markers.markers[3].pose.orientation.x, -0.961738074009, places=4)
+        self.assertAlmostEqual(markers.markers[3].pose.orientation.y, -0.0375180244707, places=4)
+        self.assertAlmostEqual(markers.markers[3].pose.orientation.z, -0.0115387773672, places=4)
+        self.assertAlmostEqual(markers.markers[3].pose.orientation.w, 0.271144115664, places=4)
+        self.assertAlmostEqual(markers.markers[3].c1.x, 129.557723999, places=4)
+        self.assertAlmostEqual(markers.markers[3].c1.y, 49.557723999, places=4)
+        self.assertAlmostEqual(markers.markers[3].c2.x, 223.442276001, places=4)
+        self.assertAlmostEqual(markers.markers[3].c2.y, 49.557723999, places=4)
+        self.assertAlmostEqual(markers.markers[3].c3.x, 223.442276001, places=4)
+        self.assertAlmostEqual(markers.markers[3].c3.y, 143.442276001, places=4)
+        self.assertAlmostEqual(markers.markers[3].c4.x, 129.557723999, places=4)
+        self.assertAlmostEqual(markers.markers[3].c4.y, 143.442276001, places=4)
 
-        assert markers.markers[1].id == 1
-        assert markers.markers[1].length == approx(0.33)
-        assert markers.markers[2].id == 4
-        assert markers.markers[2].length == approx(0.33)
+        self.assertEqual(markers.markers[1].id, 1)
+        self.assertAlmostEqual(markers.markers[1].length, 0.33, places=4)
+        self.assertEqual(markers.markers[2].id, 4)
+        self.assertAlmostEqual(markers.markers[2].length, 0.33, places=4)
 
     def test_visualization(self):
         vis = rospy.wait_for_message('aruco_detect/visualization', VisMarkerArray, timeout=5)
-        assert len(vis.markers) == 9
+        self.assertEqual(len(vis.markers), 9)
 
     def test_debug(self):
         img = rospy.wait_for_message('aruco_detect/debug', Image, timeout=5)
-        assert img.width == 640
-        assert img.height == 480
-        assert img.header.frame_id == 'main_camera_optical'
+        self.assertEqual(img.width, 640)
+        self.assertEqual(img.height, 480)
+        self.assertEqual(img.header.frame_id, 'main_camera_optical')
 
     def test_map(self):
         pose = rospy.wait_for_message('aruco_map/pose', PoseWithCovarianceStamped, timeout=5)
-        assert pose.header.frame_id == 'main_camera_optical'
-        assert pose.pose.pose.position.x == approx(-0.629167753342)
-        assert pose.pose.pose.position.y == approx(0.293822650809)
-        assert pose.pose.pose.position.z == approx(2.12641343155)
-        assert pose.pose.pose.orientation.x == approx(-0.998383794799)
-        assert pose.pose.pose.orientation.y == approx(-5.20919098575e-06)
-        assert pose.pose.pose.orientation.z == approx(-0.0300861070302)
-        assert pose.pose.pose.orientation.w == approx(0.0482143590507)
+        self.assertEqual(pose.header.frame_id, 'main_camera_optical')
+        self.assertAlmostEqual(pose.pose.pose.position.x, -0.629167753342, places=4)
+        self.assertAlmostEqual(pose.pose.pose.position.y, 0.293822650809, places=4)
+        self.assertAlmostEqual(pose.pose.pose.position.z, 2.12641343155, places=4)
+        self.assertAlmostEqual(pose.pose.pose.orientation.x, -0.998383794799, places=4)
+        self.assertAlmostEqual(pose.pose.pose.orientation.y, -5.20919098575e-06, places=4)
+        self.assertAlmostEqual(pose.pose.pose.orientation.z, -0.0300861070302, places=4)
+        self.assertAlmostEqual(pose.pose.pose.orientation.w, 0.0482143590507, places=4)
 
     def test_map_image(self):
         img = rospy.wait_for_message('aruco_map/image', Image, timeout=5)
-        assert img.width == 2000
-        assert img.height == 2000
-        assert img.encoding == 'mono8'
+        self.assertEqual(img.width, 2000)
+        self.assertEqual(img.height, 2000)
+        self.assertEqual(img.encoding, 'mono8')
 
     def test_map_visualization(self):
         vis = rospy.wait_for_message('aruco_map/visualization', VisMarkerArray, timeout=5)

aruco_pose/test/basic.txt

@@ -3,3 +3,5 @@
 3	0.33	0	1	0	0	0	0
 4	0.33	1	1	0	0	0	0
 10	0.5	0.5	2	0	1.2	0	0
+11	0.2	0.5	0.5	0	0.0	-1	1
+12	0.4	0.2	0.5	0	0.1	-1.2	-0.5

builder/assets/kinetic-rosdep-clever.yaml

@@ -537,4 +537,7 @@ interactive_marker_proxy:
     stretch: [ros-kinetic-interactive-marker-proxy]
 tf2_web_republisher:
   debian:
-    stretch: [ros-kinetic-tf2-web-republisher]
+    stretch: [ros-kinetic-tf2-web-republisher]
+image_publisher:
+  debian:
+    stretch: [ros-kinetic-image-publisher]

builder/image-ros.sh

@@ -78,7 +78,7 @@ resolve_rosdep() {
 
   echo_stamp "Installing dependencies apps with rosdep in ${CATKIN_PATH}"
   cd ${CATKIN_PATH}
-  my_travis_retry rosdep install -y --from-paths src --ignore-src --rosdistro ${ROS_DISTRO} -r --os=${OS_DISTRO}:${OS_VERSION}
+  my_travis_retry rosdep install -y --from-paths src --ignore-src --rosdistro ${ROS_DISTRO} --os=${OS_DISTRO}:${OS_VERSION}
 }
 
 INSTALL_ROS_PACK_SOURCES=${INSTALL_ROS_PACK_SOURCES:='false'}
@@ -135,6 +135,8 @@ if [ "${INSTALL_ROS_PACK_SOURCES}" = "true" ]; then
   chown -Rf pi:pi /home/pi/ros_catkin_ws
 fi
 
+export ROS_IP='127.0.0.1' # needed for running tests
+
 echo_stamp "Installing CLEVER" \
 && cd /home/pi/catkin_ws/src/clever \
 && git status \
@@ -144,6 +146,8 @@ echo_stamp "Installing CLEVER" \
 && my_travis_retry pip install -r /home/pi/catkin_ws/src/clever/clever/requirements.txt \
 && source /opt/ros/kinetic/setup.bash \
 && catkin_make -j2 -DCMAKE_BUILD_TYPE=Release \
+&& catkin_make run_tests \
+&& catkin_test_results \
 && systemctl enable roscore \
 && systemctl enable clever \
 && echo_stamp "All CLEVER was installed!" "SUCCESS" \

builder/image-software.sh

@@ -121,8 +121,10 @@ pip --version
 pip3 --version
 
 echo_stamp "Install and enable Butterfly (web terminal)"
-my_travis_retry pip3 install --prefer-binary butterfly
-my_travis_retry pip3 install --prefer-binary butterfly[systemd]
+echo_stamp "Workaround for tornado >= 6.0 breaking butterfly"
+my_travis_retry pip3 install tornado==5.1.1
+my_travis_retry pip3 install butterfly
+my_travis_retry pip3 install butterfly[systemd]
 systemctl enable butterfly.socket
 
 echo_stamp "Install ws281x library"
@@ -139,6 +141,7 @@ wget https://nodejs.org/dist/v10.15.0/node-v10.15.0-linux-armv6l.tar.gz
 tar -xzf node-v10.15.0-linux-armv6l.tar.gz
 cp -R node-v10.15.0-linux-armv6l/* /usr/local/
 rm -rf node-v10.15.0-linux-armv6l/
+rm node-v10.15.0-linux-armv6l.tar.gz
 
 echo_stamp "Add .vimrc"
 cat << EOF > /home/pi/.vimrc

builder/test/tests.sh

@@ -44,3 +44,5 @@ rosversion compressed_image_transport
 rosversion rosbridge_suite
 rosversion rosserial
 rosversion usb_cam
+rosversion cv_camera
+rosversion web_video_server

clever/launch/aruco.launch

@@ -3,6 +3,8 @@
     <arg name="aruco_map" default="false"/>
     <arg name="aruco_vpe" default="false"/>
 
+    <!-- For additional help go to https://clever.copterexpress.com/aruco.html -->
+
     <!-- aruco_detect: detect aruco markers, estimate poses -->
     <node name="aruco_detect" pkg="nodelet" if="$(arg aruco_detect)" type="nodelet" args="load aruco_pose/aruco_detect nodelet_manager" output="screen" clear_params="true">
         <remap from="image_raw" to="main_camera/image_raw"/>

clever/launch/main_camera.launch

@@ -21,11 +21,11 @@
         <param name="frame_id" value="main_camera_optical"/>
         <param name="camera_info_url" value="file://$(find clever)/camera_info/fisheye_cam_320.yaml"/>
 
-        <!-- setting camera FPS -->
-        <param name="rate" value="100"/>
-        <param name="cv_cap_prop_fps" value="40"/>
-        <param name="capture_delay" value="0.02"/>
+        <param name="rate" value="100"/> <!-- poll rate -->
+        <param name="cv_cap_prop_fps" value="40"/> <!-- camera FPS -->
+        <param name="capture_delay" value="0.02"/> <!-- approximate delay on frame retrieving -->
 
+        <!-- camera resolution, NOTE: camera_info file should match it -->
         <param name="image_width" value="320"/>
         <param name="image_height" value="240"/>
     </node>

clever/launch/mavros.launch

@@ -27,26 +27,28 @@
         <!-- basic params -->
         <rosparam command="load" file="$(find clever)/launch/mavros_config.yaml"/>
 
-        <rosparam param="plugin_blacklist">
-            - safety_area
-            - image_pub
-            - vibration
-            - rangefinder
-            - 3dr_radio
-            - actuator_control
-            - hil_controls
-            - vfr_hud
-            - vision_speed_estimate
-            - fake_gps
-            - cam_imu_sync
-            - hil
-            - adsb
-            - waypoint
-            - obstacle_distance
-            - setpoint_accel
-            - trajectory
-            - wind_estimation
-            - home_position
+        <rosparam param="plugin_whitelist">
+            - altitude
+            - command
+            - distance_sensor
+            - ftp
+            - global_position
+            - imu
+            - local_position
+            - manual_control
+            # - mocap_pose_estimate
+            - param
+            - px4flow
+            - rc_io
+            - setpoint_attitude
+            - setpoint_position
+            - setpoint_raw
+            - setpoint_velocity
+            - sys_status
+            - sys_time
+            - vision_pose_estimate
+            # - vision_speed_estimate
+            # - waypoint
         </rosparam>
     </node>
 

clever/requirements.txt

@@ -1,4 +1,5 @@
 flask==0.12.3
+docopt==0.6.2
 geopy==1.11.0
 pymavlink==2.2.10
 smbus2==0.2.1

clever/src/selfcheck.py

@@ -43,7 +43,7 @@ def check(name):
                     rospy.logwarn('%s: %s', name, f)
             except Exception as e:
                 traceback.print_exc()
-                rospy.logwarn('%s: exception occured', name)
+                rospy.logwarn('%s: exception occurred', name)
                 return
             if not failures:
                 rospy.loginfo('%s: OK', name)
@@ -80,12 +80,17 @@ def check_camera(name):
         failure('%s: calibration height doesn\'t match image height (%d != %d))', name, info.height, img.height)
 
 
-@check('Aruco detector')
+@check('ArUco detector')
 def check_aruco():
     try:
-        rospy.wait_for_message('aruco_pose/debug', Image, timeout=1)
+        rospy.wait_for_message('aruco_detect/markers', MarkerArray, timeout=1)
     except rospy.ROSException:
-        failure('no aruco_pose/debug messages')
+        failure('no markers detection')
+        return
+    try:
+        rospy.wait_for_message('aruco_map/pose', PoseWithCovarianceStamped, timeout=1)
+    except rospy.ROSException:
+        failure('no map detection')
 
 
 @check('Vision position estimate')

docs/ru/aruco_map.md

@@ -6,9 +6,10 @@
 
 ## Конфигурирование
 
-Для включения распознавания карт маркеров аргумент `aruco_map` в файле `~/catkin_ws/src/clever/clever/launch/aruco.launch` должен быть в значении `true`:
+Для включения распознавания карт маркеров аргументы `aruco_map` и `aruco_detect` в файле `~/catkin_ws/src/clever/clever/launch/aruco.launch` должны быть в значении `true`:
 
 ```xml
+<arg name="aruco_detect" default="true"/>
 <arg name="aruco_map" default="true"/>
 ```
 
@@ -36,6 +37,20 @@ id_маркера размер_маркера x y z угол_z угол_y уго
 
 Смотрите примеры карт маркеров в каталоге [`~/catkin_ws/src/clever/aruco_pose/map`](https://github.com/CopterExpress/clever/tree/master/aruco_pose/map).
 
+Файл карты может быть сгенерирован с помощью инструмента `genmap.py`:
+
+```bash
+rosrun aruco_pose genmap.py length x y dist_x dist_y first > ~/catkin_ws/src/clever/aruco_pose/map/test_map.txt
+```
+
+Где `length` – размер маркера, `x` – количество маркеров по оси *x*, `y` - количество маркеров по оси *y*, `dist_x` – расстояние между центрами маркеров по оси *x*, `y` – расстояние между центрами маркеров по оси *y*, `first` – ID первого (левого нижнего) маркера, `test_map.txt` – название файла с картой. Дополнительный ключ `--top-left` позволяет нумеровать маркеры с левого верхнего угла.
+
+Пример:
+
+```bash
+rosrun aruco_pose genmap.py 0.33 2 4 1 1 0 > ~/catkin_ws/src/clever/aruco_pose/map/test_map.txt
+```
+
 ### Проверка
 
 Для контроля карты, по которой в данный момент коптер осуществляет навигацию, можно просмотреть содержимое топика `/aruco_map/image`. Через браузер его можно просмотреть при помощи [web_video_server](web_video_server.md) по ссылке http://192.168.11.1:8080/snapshot?topic=/aruco_map/image:
@@ -99,8 +114,12 @@ navigate(2, 2, 2, speed=1, frame_id='aruco_map')  # полет в координ
 
 ## Дополнительные настройки
 
+<!-- TODO: статья по пидам -->
+
 Если коптер нестабильно удерживает позицию по VPE, попробуйте увеличить коэффициенты *P* PID-регулятора по скорости – параметры `MPC_XY_VEL_P` и `MPC_Z_VEL_P`.
 
+Если коптер нестабильно удерживает высоту, попробуйте увеличить коэффициент `MPC_Z_VEL_P` или лучше подобрать газ висения – `MPC_THR_HOVER`.
+
 ## Расположение маркеров на потолке
 
 ![Маркеры на потолке](../assets/IMG_4175.JPG)

docs/ru/frames.md

@@ -14,3 +14,14 @@
 > **Hint** В соответствии с [соглашением](http://www.ros.org/reps/rep-0103.html), для фреймов, связанных с коптером, ось X направлена вперед, Y – налево и Z – вверх.
 
 Более наглядно 3D визуализацию систем координат можно наблюдать, используя [rviz](rviz.md).
+
+tf2
+--
+
+Основная документация: http://wiki.ros.org/tf2
+
+Для работы с системами координат в Клевере используется ROS-пакет tf2. tf2 – это набор библиотек для языков программирования C++, Python и других, которые помогают работать с системами координат. ROS-ноды публикуют в топик `/tf` сообщения формата `TransformStamped`, которые содержат в себе трансформации между заданными системами координат в определенные моменты времени.
+
+С помощью [`simple_offboard`](simple_offboard.md) можно запросить расположение коптера в любой системе координат, используя аргумент `frame_id` сервиса `get_telemetry`.
+
+Из Python можно использовать библиотеку tf2 для преобразования геометрических объектов (например, PoseStamped, PointStamped) из одной системы координат в другую.

docs/ru/optical_flow.md

@@ -20,14 +20,22 @@
 
 ## Настройка полетного контроллера
 
-Рекомендуемые параметры PX4:
+При использовании **EKF2** (параметр `SYS_MC_EST_GROUP` = `ekf2`):
 
-* `SYS_MC_EST_GROUP` – 2 (EKF2).
-* `EKF2_AID_MASK` – use optical flow.
+* `EKF2_AID_MASK` – включен флажок use optical flow.
 * `EKF2_OF_DELAY` – 0.
-* `EKF2_OF_QMIN` – 20.
+* `EKF2_OF_QMIN` – 15.
 * `SENS_FLOW_ROT` – No rotation (отсутствие поворота).
-* `EKF2_HGT_MODE` – range sensor (см. [конфигурирование дальномера](laser.md)).
+* Опционально: `EKF2_HGT_MODE` – range sensor (см. [конфигурирование дальномера](laser.md)).
+
+При использовании **LPE** (параметр `SYS_MC_EST_GROUP` = `local_position_estimator, attitude_estimator_q`):
+
+* `LPE_FUSION` – включены флажки fuse optical flow и flow gyro compensation.
+* `EKF2_OF_DELAY` – 0.
+* `LPE_FLW_QMIN` – 15.
+* `LPE_FLW_SCALE` – 1.0.
+* `SENS_FLOW_ROT` – No rotation (отсутствие поворота).
+* Опционально: `LPE_FUSION` – включен флажок pub agl as lpos down (см. [конфигурирование дальномера](laser.md).
 
 ## Полет в POSCTL
 
@@ -49,14 +57,15 @@ navigate(z=1.5, frame_id='body', auto_arm=True)
 navigate(x=1.5, frame_id='body')
 ```
 
-## Неисправности
+При использовании Optical Flow возможна также [навигация по ArUco-маркерам](aruco_marker.md).
 
-При появлении в QGC ошибок типа `EKF INTERNAL CHECKS` попробуйте перезагрузить EKF2. Для этого наберите в MAVLink-консоли:
+## Дополнительные настройки
 
-```nsh
-ekf2 stop
-ekf2 start
-```
+<!-- TODO: статья по пидам -->
+
+Если коптер нестабильно удерживает позицию по VPE, попробуйте увеличить коэффициенты *P* PID-регулятора по скорости – параметры `MPC_XY_VEL_P` и `MPC_Z_VEL_P`.
+
+Если коптер нестабильно удерживает высоту, попробуйте увеличить коэффициент `MPC_Z_VEL_P` или лучше подобрать газ висения – `MPC_THR_HOVER`.
 
 Если коптер сильно уплывает по рысканью, попробуйте:
 
@@ -67,5 +76,15 @@ ekf2 start
 
 Если коптер уплывает по высоте, попробуйте:
 
-* Изменить значение параметра `MPC_THR_HOVER`;
+* повысить значение коэффициента `MPC_Z_VEL_P`;
+* изменить значение параметра `MPC_THR_HOVER`;
 * выставить `MPC_ALT_MODE` = 2 (Terrain following).
+
+## Неисправности
+
+При появлении в QGC ошибок типа `EKF INTERNAL CHECKS` попробуйте перезагрузить EKF2. Для этого наберите в MAVLink-консоли:
+
+```nsh
+ekf2 stop
+ekf2 start
+```