docs: add article about using sonar

Auto commit by GitBook Editor

.gitignore

@@ -1,3 +1,6 @@
 *.pyc
 *.DS_Store
-/images
+/images
+node_modules/
+_book/
+package-lock.json

.markdownlint.json

@@ -1,8 +1,16 @@
 {
     "MD003": false,
+    "MD010": {
+        "code_blocks": false
+    },
     "MD013": false,
+    "MD024": false,
+    "MD026" :{
+        "punctuation": ".,;:!"
+    },
     "MD033": false,
     "MD034": false,
+    "MD040": false,
     "MD044": {
         "names": [
             "MAVLink",
@@ -18,5 +26,6 @@
             "ArUco"
         ],
         "code_blocks": false
-    }
+    },
+    "MD045": false
 }

.travis.yml

@@ -4,7 +4,7 @@ services:
   - docker
 env:
   global:
-    - DOCKER="smirart/img-tool:v0.1"
+    - DOCKER="goldarte/img-tool:builder-mod"
     - TARGET_REPO="https://github.com/${TRAVIS_REPO_SLUG}.git"
     - 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"
@@ -16,8 +16,8 @@ script:
   - docker run --privileged --rm -v /dev:/dev -v $(pwd):/builder/repo -e TRAVIS_TAG="${TRAVIS_TAG}" ${DOCKER}
 before_deploy:
   # Set up git user name and tag this commit
-  - git config --local user.name "urpylka"
-  - git config --local user.email "urpylka@gmail.com"
+  - git config --local user.name "goldarte"
+  - git config --local user.email "goldartt@gmail.com"
   - sudo chmod -R 777 *
   - cd images && zip ${IMAGE_NAME}.zip ${IMAGE_NAME}
 deploy:

README.md

@@ -14,7 +14,7 @@ Use it to learn how to assemble, configure, pilot and program autonomous CLEVER
 
 **Preconfigured image for Raspberry Pi 3 with installed and configured software, ready to fly, is available [in the Releases section](https://github.com/CopterExpress/clever/releases).**
 
-[![Build Status](https://travis-ci.org/urpylka/clever.svg?branch=master)](https://travis-ci.org/urpylka/clever)
+[![Build Status](https://travis-ci.org/CopterExpress/clever.svg?branch=master)](https://travis-ci.org/CopterExpress/clever)
 
 Image includes:
 
@@ -25,7 +25,7 @@ Image includes:
 * mavros
 * CLEVER software bundle for autonomous drone control
 
-API description (in Russian) for autonomous flights is available [on GitBook](https://copterexpress.gitbooks.io/clever/simple_offboard.html).
+API description (in Russian) for autonomous flights is available [on GitBook](https://clever.copterexpress.com/simple_offboard.html).
 
 ## Manual installation
 

apps/ios/cleverrc/telemetry.js

@@ -64,9 +64,19 @@ new ROSLIB.Topic({
 var notificationHideTimer;
 
 function notify(text, severity) {
+	var repeated = notificationsEl.querySelector('.item:first-of-type[data-text=' + text + ']');
+	if (repeated) {
+		// don't repeat notifications
+		var count = repeated.getAttribute('data-count') || 1;
+		repeated.setAttribute('data-count', ++count);
+		repeated.innerHTML = text + ' (' + count + ')';
+		return;
+	}
+
 	var item = document.createElement('div');
 	item.innerHTML = text;
 	item.classList.add('item');
+	item.setAttribute('data-text', text);
 	notificationsEl.prepend(item);
 	var itemHeight = item.offsetHeight;
 	notificationsEl.classList.remove('anim');

aruco_pose/CMakeLists.txt

@@ -16,15 +16,11 @@ find_package(catkin REQUIRED COMPONENTS
   image_transport
   cv_bridge
   tf
-  tf2
-  tf2_ros
-  tf2_geometry_msgs
-  sensor_msgs
-  message_generation
+  #tf2
+  #tf2_ros
+  #aruco_msgs
 )
 
-find_package(OpenCV REQUIRED)
-
 ## System dependencies are found with CMake's conventions
 # find_package(Boost REQUIRED COMPONENTS system)
 
@@ -59,12 +55,11 @@ find_package(OpenCV REQUIRED)
 ##   * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...)
 
 ## Generate messages in the 'msg' folder
-add_message_files(
-  FILES
-  Point2D.msg
-  Marker.msg
-  MarkerArray.msg
-)
+#add_message_files(
+#  FILES
+#  Marker.msg
+#  MarkerArray.msg
+#)
 
 ## Generate services in the 'srv' folder
 # add_service_files(
@@ -81,11 +76,10 @@ add_message_files(
 # )
 
 ## Generate added messages and services with any dependencies listed here
-generate_messages(
-  DEPENDENCIES
-  std_msgs
-  geometry_msgs
-)
+#generate_messages(
+#  DEPENDENCIES
+#  std_msgs  # Or other packages containing msgs
+#)
 
 ################################################
 ## Declare ROS dynamic reconfigure parameters ##
@@ -117,9 +111,9 @@ generate_messages(
 ## CATKIN_DEPENDS: catkin_packages dependent projects also need
 ## DEPENDS: system dependencies of this project that dependent projects also need
 catkin_package(
-  INCLUDE_DIRS DEPENDS OpenCV
+#  INCLUDE_DIRS include
   LIBRARIES aruco_pose
-  CATKIN_DEPENDS message_runtime
+#  CATKIN_DEPENDS other_catkin_pkg
 #  DEPENDS system_lib
 )
 
@@ -132,13 +126,11 @@ catkin_package(
 include_directories(
 # include
   ${catkin_INCLUDE_DIRS}
-  ${OpenCV_INCLUDE_DIRS}
 )
 
 ## Declare a C++ library
-add_library(aruco_pose
-  src/aruco_detect.cpp
-  src/aruco_map.cpp
+add_library(${PROJECT_NAME}
+  src/aruco_pose.cpp
 )
 
 ## Add cmake target dependencies of the library
@@ -162,9 +154,11 @@ add_library(aruco_pose
 # add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
 
 ## Specify libraries to link a library or executable target against
-target_link_libraries(aruco_pose
+link_directories(/opt/ros/kinetic/lib)
+
+target_link_libraries(${PROJECT_NAME}
   ${catkin_LIBRARIES}
-  ${OpenCV_LIBS}
+  "/opt/ros/kinetic/lib/libopencv_aruco3.so" # TODO: fix launch fails with .so loading
 )
 
 #############

aruco_pose/map/map.txt

@@ -1,4 +0,0 @@
-1	0.33	0	0	0	0	0	0
-2	0.33	1	0	0	0	0	0
-3	0.33	0	1	0	0	0	0
-4	0.33	1	1	0	0	0	0

aruco_pose/msg/Marker.msg

@@ -1,6 +0,0 @@
-uint32 id
-geometry_msgs/PoseWithCovariance pose
-Point2D c1
-Point2D c2
-Point2D c3
-Point2D c4

aruco_pose/msg/MarkerArray.msg

@@ -1,2 +0,0 @@
-Header header
-Marker[] markers

aruco_pose/msg/Point2D.msg

@@ -1,2 +0,0 @@
-float32 x
-float32 y

aruco_pose/nodelet_plugins.xml

@@ -1,8 +1,5 @@
 <library path="lib/libaruco_pose">
-   <class name="aruco_pose/aruco_detect" type="ArucoDetect" base_class_type="nodelet::Nodelet">
-      <description/>
-   </class>
-   <class name="aruco_pose/aruco_map" type="ArucoMap" base_class_type="nodelet::Nodelet">
+   <class name="aruco_pose/aruco_pose" type="ArucoPose" base_class_type="nodelet::Nodelet">
       <description/>
    </class>
 </library>

aruco_pose/package.xml

@@ -2,7 +2,7 @@
 <package>
   <name>aruco_pose</name>
   <version>0.0.0</version>
-  <description>Positioning by ArUco markers</description>
+  <description>ArUco maps precise pose estimation nodelet</description>
 
   <maintainer email="okalachev@gmail.com">Oleg Kalachev</maintainer>
   <license>MIT</license>
@@ -11,30 +11,21 @@
   <author email="okalachev@gmail.com">Oleg Kalachev</author>
   <author email="urpylka@gmail.com">Artem Smirnov</author>
 
-  <buildtool_depend>catkin</buildtool_depend>
-
-  <build_depend>tf</build_depend>
-  <build_depend>tf2</build_depend>
-  <build_depend>tf2_ros</build_depend>
-  <build_depend>tf2_geometry_msgs</build_depend>
-  <build_depend>cv_bridge</build_depend>
-  <build_depend>dynamic_reconfigure</build_depend>
-  <build_depend>image_transport</build_depend>
-  <build_depend>message_generation</build_depend>
-  <build_depend>message_runtime</build_depend>
+  <!-- Use build_depend for packages you need at compile time: -->
   <build_depend>nodelet</build_depend>
   <build_depend>roscpp</build_depend>
-  <build_depend>std_msgs</build_depend>
+  <build_depend>image_transport</build_depend>
+  <build_depend>cv_bridge</build_depend>
+  <build_depend>tf</build_depend>
+  <!-- Use buildtool_depend for build tool packages: -->
+  <buildtool_depend>catkin</buildtool_depend>
+  <!-- Use run_depend for packages you need at runtime: -->
   <run_depend>nodelet</run_depend>
   <run_depend>roscpp</run_depend>
   <run_depend>image_transport</run_depend>
   <run_depend>cv_bridge</run_depend>
-  <run_depend>message_runtime</run_depend>
-  <run_depend>std_msgs</run_depend>
-  <run_depend>tf</run_depend>
-  <run_depend>tf2</run_depend>
-  <run_depend>tf2_ros</run_depend>
-  <run_depend>tf2_geometry_msgs</run_depend>
+  <!-- Use test_depend for packages you need only for testing: -->
+  <!--   <test_depend>gtest</test_depend> -->
 
   <!-- The export tag contains other, unspecified, tags -->
   <export>

aruco_pose/src/aruco_detect.cpp

@@ -1,293 +0,0 @@
-/*
- * Detecting and positioning ArUco markers
- * Copyright (C) 2018 Copter Express Technologies
- *
- * Author: Oleg Kalachev <okalachev@gmail.com>
- *
- * Distributed under MIT License (available at https://opensource.org/licenses/MIT).
- * The above copyright notice and this permission notice shall be included in all
- * copies or substantial portions of the Software.
- */
-
-/*
- * Code is based on https://github.com/UbiquityRobotics/fiducials, which is distributed
- * under the BSD license.
- */
-
-#include <math.h>
-#include <vector>
-#include <string>
-#include <ros/ros.h>
-#include <nodelet/nodelet.h>
-#include <pluginlib/class_list_macros.h>
-#include <tf/transform_datatypes.h>
-#include <tf2_ros/buffer.h>
-#include <tf2_ros/transform_listener.h>
-#include <tf2_ros/transform_broadcaster.h>
-#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
-#include <image_transport/image_transport.h>
-#include <cv_bridge/cv_bridge.h>
-#include <geometry_msgs/Vector3.h>
-#include <geometry_msgs/Pose.h>
-#include <geometry_msgs/PoseStamped.h>
-#include <geometry_msgs/PoseWithCovarianceStamped.h>
-#include <geometry_msgs/TransformStamped.h>
-#include <visualization_msgs/Marker.h>
-#include <visualization_msgs/MarkerArray.h>
-
-#include <opencv2/opencv.hpp>
-#include <opencv2/highgui.hpp>
-#include <opencv2/aruco.hpp>
-#include <opencv2/imgproc/imgproc.hpp>
-#include <opencv2/calib3d/calib3d.hpp>
-
-#include <aruco_pose/Marker.h>
-#include <aruco_pose/MarkerArray.h>
-
-#include "utils.h"
-
-using std::vector;
-using cv::Mat;
-
-class ArucoDetect : public nodelet::Nodelet {
-private:
-	ros::NodeHandle nh_, nh_priv_;
-	tf2_ros::TransformBroadcaster br_;
-	tf2_ros::Buffer tf_buffer_;
-	tf2_ros::TransformListener tf_listener_{tf_buffer_};
-	cv::Ptr<cv::aruco::Dictionary> dictionary_;
-	cv::Ptr<cv::aruco::DetectorParameters> parameters_;
-	image_transport::Publisher debug_pub_;
-	image_transport::CameraSubscriber img_sub_;
-	ros::Publisher markers_pub_, vis_markers_pub_;
-	bool estimate_poses_, send_tf_;
-	double length_;
-	std::string snap_orientation_;
-	Mat camera_matrix_, dist_coeffs_;
-	aruco_pose::MarkerArray array_;
-	visualization_msgs::MarkerArray vis_array_;
-
-public:
-	virtual void onInit()
-	{
-		nh_ = getNodeHandle();
-		nh_priv_ = getPrivateNodeHandle();
-
-		int dictionary;
-		nh_priv_.param("dictionary", dictionary, 2);
-		nh_priv_.param("estimate_poses", estimate_poses_, true);
-		nh_priv_.param("send_tf", send_tf_, true);
-		if (estimate_poses_ && !nh_priv_.getParam("length", length_)) {
-			ROS_FATAL("aruco_detect: can't estimate marker's poses as ~length parameter is not defined");
-			ros::shutdown();
-		}
-		nh_priv_.param<std::string>("snap_orientation", snap_orientation_, "");
-
-		camera_matrix_ = cv::Mat::zeros(3, 3, CV_64F);
-		dist_coeffs_ = cv::Mat::zeros(8, 1, CV_64F);
-
-		dictionary_ = cv::aruco::getPredefinedDictionary(static_cast<cv::aruco::PREDEFINED_DICTIONARY_NAME>(dictionary));
-		parameters_ = cv::aruco::DetectorParameters::create();
-		parameters_->cornerRefinementMethod = cv::aruco::CORNER_REFINE_SUBPIX;
-
-		image_transport::ImageTransport it(nh_);
-		image_transport::ImageTransport it_priv(nh_priv_);
-
-		debug_pub_ = it_priv.advertise("debug", 1);
-		markers_pub_ = nh_priv_.advertise<aruco_pose::MarkerArray>("markers", 1);
-		vis_markers_pub_ = nh_priv_.advertise<visualization_msgs::MarkerArray>("visualization", 1);
-		img_sub_ = it.subscribeCamera("image_raw", 1, &ArucoDetect::imageCallback, this);
-
-		ROS_INFO("aruco_detect: ready");
-	}
-
-private:
-	void imageCallback(const sensor_msgs::ImageConstPtr& msg, const sensor_msgs::CameraInfoConstPtr &cinfo)
-	{
-		Mat image = cv_bridge::toCvShare(msg, "bgr8")->image;
-
-		vector<int> ids;
-		vector<vector<cv::Point2f>> corners, rejected;
-		vector<cv::Vec3d> rvecs, tvecs;
-		vector<cv::Point3f> obj_points;
-		geometry_msgs::TransformStamped snap_to;
-
-		// Detect markers
-		cv::aruco::detectMarkers(image, dictionary_, corners, ids, parameters_, rejected);
-
-		array_.header.stamp = msg->header.stamp;
-		array_.header.frame_id = msg->header.frame_id;
-		array_.markers.clear();
-
-		if (ids.size() != 0) {
-			parseCameraInfo(cinfo, camera_matrix_, dist_coeffs_);
-
-			// Estimate individual markers' poses
-			if (estimate_poses_) {
-				cv::aruco::estimatePoseSingleMarkers(corners, length_, camera_matrix_, dist_coeffs_,
-				                                     rvecs, tvecs);
-
-				if (!snap_orientation_.empty()) {
-					try {
-						snap_to = tf_buffer_.lookupTransform(msg->header.frame_id, snap_orientation_,
-						                                     msg->header.stamp, ros::Duration(0.02));
-					} catch (const tf2::TransformException& e) {
-						ROS_WARN_THROTTLE(5, "aruco_detect: can't snap: %s", e.what());
-					}
-				}
-			}
-
-			array_.markers.reserve(ids.size());
-			aruco_pose::Marker marker;
-			geometry_msgs::TransformStamped transform;
-			transform.header.stamp = msg->header.stamp;
-			transform.header.frame_id = msg->header.frame_id;
-
-			for (unsigned int i = 0; i < ids.size(); i++) {
-				marker.id = ids[i];
-				fillCorners(marker, corners[i]);
-
-				if (estimate_poses_) {
-					fillPose(marker.pose.pose, rvecs[i], tvecs[i]);
-
-					// snap orientation (if enabled and snap frame avaiable)
-					if (!snap_orientation_.empty() && !snap_to.header.frame_id.empty()) {
-						snapOrientation(marker.pose.pose, snap_to.transform.rotation);
-					}
-
-					// TODO: check IDs are unique
-					if (send_tf_) {
-						transform.child_frame_id = getChildFrameId(ids[i]);
-						transform.transform.rotation = marker.pose.pose.orientation;
-						fillTranslation(transform.transform.translation, tvecs[i]);
-						br_.sendTransform(transform);
-					}
-				}
-				array_.markers.push_back(marker);
-			}
-		}
-
-		markers_pub_.publish(array_);
-
-		// Publish visualization markers
-		if (estimate_poses_ && vis_markers_pub_.getNumSubscribers() != 0) {
-			// Delete all markers
-			visualization_msgs::Marker vis_marker;
-			vis_marker.action = visualization_msgs::Marker::DELETEALL;
-			vis_array_.markers.clear();
-			vis_array_.markers.reserve(ids.size() + 1);
-			vis_array_.markers.push_back(vis_marker);
-
-			for (unsigned int i = 0; i < ids.size(); i++)
-				pushVisMarkers(msg->header.frame_id, msg->header.stamp, array_.markers[i].pose.pose,
-				               length_, ids[i], i);
-
-			vis_markers_pub_.publish(vis_array_);
-		}
-
-		// Publish debug image
-		if (debug_pub_.getNumSubscribers() != 0) {
-			Mat debug = image.clone();
-			cv::aruco::drawDetectedMarkers(debug, corners, ids); // draw markers
-			if (estimate_poses_)
-				for (unsigned int i = 0; i < ids.size(); i++)
-					cv::aruco::drawAxis(debug, camera_matrix_, dist_coeffs_, rvecs[i], tvecs[i], length_);
-
-			cv_bridge::CvImage out_msg;
-			out_msg.header.frame_id = msg->header.frame_id;
-			out_msg.header.stamp = msg->header.stamp;
-			out_msg.encoding = sensor_msgs::image_encodings::BGR8;
-			out_msg.image = debug;
-			debug_pub_.publish(out_msg.toImageMsg());
-		}
-	}
-
-	inline void fillCorners(aruco_pose::Marker& marker, const vector<cv::Point2f>& corners) const
-	{
-		marker.c1.x = corners[0].x;
-		marker.c2.x = corners[1].x;
-		marker.c3.x = corners[2].x;
-		marker.c4.x = corners[3].x;
-		marker.c1.y = corners[0].y;
-		marker.c2.y = corners[1].y;
-		marker.c3.y = corners[2].y;
-		marker.c4.y = corners[3].y;
-	}
-
-	inline void fillPose(geometry_msgs::Pose& pose, const cv::Vec3d& rvec, const cv::Vec3d& tvec) const
-	{
-		pose.position.x = tvec[0];
-		pose.position.y = tvec[1];
-		pose.position.z = tvec[2];
-
-		double angle = norm(rvec);
-		cv::Vec3d axis = rvec / angle;
-
-		tf2::Quaternion q;
-		q.setRotation(tf2::Vector3(axis[0], axis[1], axis[2]), angle);
-
-		pose.orientation.w = q.w();
-		pose.orientation.x = q.x();
-		pose.orientation.y = q.y();
-		pose.orientation.z = q.z();
-	}
-
-	inline void fillTranslation(geometry_msgs::Vector3& translation, const cv::Vec3d& tvec) const
-	{
-		translation.x = tvec[0];
-		translation.y = tvec[1];
-		translation.z = tvec[2];
-	}
-
-	void pushVisMarkers(const std::string& frame_id, const ros::Time& stamp,
-	                    const geometry_msgs::Pose &pose, double length, int id, int index)
-	{
-		visualization_msgs::Marker marker;
-		marker.header.frame_id = frame_id;
-		marker.header.stamp = stamp;
-		marker.action = visualization_msgs::Marker::ADD;
-		marker.id = index;
-
-		// Marker
-		marker.ns = "aruco_marker";
-		marker.type = visualization_msgs::Marker::CUBE;
-		marker.scale.x = length;
-		marker.scale.y = length;
-		marker.scale.z = 0.001;
-		marker.color.r = 1;
-		marker.color.g = 1;
-		marker.color.b = 1;
-		marker.color.a = 0.9;
-		marker.pose = pose;
-		vis_array_.markers.push_back(marker);
-
-		// Label
-		marker.ns = "aruco_marker_label";
-		marker.type = visualization_msgs::Marker::TEXT_VIEW_FACING;
-		marker.scale.z = length * 0.6;
-		marker.color.r = 0;
-		marker.color.g = 0;
-		marker.color.b = 0;
-		marker.color.a = 1;
-		marker.text = std::to_string(id);
-		marker.pose = pose;
-		vis_array_.markers.push_back(marker);
-	}
-
-	void snapOrientation(geometry_msgs::Pose& pose, const geometry_msgs::Quaternion orientation)
-	{
-		tf::Quaternion q;
-		q.setRPY(0, 0, -tf::getYaw(pose.orientation) + tf::getYaw(orientation) + M_PI / 2);
-		tf::Quaternion pq;
-		tf::quaternionMsgToTF(orientation, pq);
-		pq = pq * q;
-		tf::quaternionTFToMsg(pq, pose.orientation);
-	}
-
-	inline std::string getChildFrameId(int id) const
-	{
-		return "aruco_" + std::to_string(id);
-	}
-};
-
-PLUGINLIB_EXPORT_CLASS(ArucoDetect, nodelet::Nodelet)

aruco_pose/src/aruco_map.cpp

@@ -1,307 +0,0 @@
-/*
- * Positioning ArUco markers maps
- * Copyright (C) 2018 Copter Express Technologies
- *
- * Author: Oleg Kalachev <okalachev@gmail.com>
- *
- * Distributed under MIT License (available at https://opensource.org/licenses/MIT).
- * The above copyright notice and this permission notice shall be included in all
- * copies or substantial portions of the Software.
- */
-
-/*
- * Code is based on https://github.com/UbiquityRobotics/fiducials, which is distributed
- * under the BSD license.
- */
-
-#include <math.h>
-#include <string>
-#include <vector>
-#include <fstream>
-#include <ros/ros.h>
-#include <nodelet/nodelet.h>
-#include <pluginlib/class_list_macros.h>
-#include <image_transport/image_transport.h>
-#include <cv_bridge/cv_bridge.h>
-#include <tf2_ros/buffer.h>
-#include <tf2_ros/transform_listener.h>
-#include <tf2_ros/transform_broadcaster.h>
-#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
-#include <geometry_msgs/TransformStamped.h>
-#include <geometry_msgs/PoseWithCovarianceStamped.h>
-#include <sensor_msgs/Image.h>
-#include <visualization_msgs/Marker.h>
-#include <visualization_msgs/MarkerArray.h>
-
-#include <aruco_pose/MarkerArray.h>
-#include <aruco_pose/Marker.h>
-
-#include <opencv2/opencv.hpp>
-#include <opencv2/aruco.hpp>
-
-#include "utils.h"
-#include "gridboard.h"
-
-using std::vector;
-using cv::Mat;
-
-class ArucoMap : public nodelet::Nodelet {
-private:
-	ros::NodeHandle nh_, nh_priv_;
-	ros::Publisher img_pub_, pose_pub_;
-	ros::Subscriber markers_sub_, cinfo_sub;
-	cv::Ptr<cv::aruco::Board> board_;
-	Mat camera_matrix_, dist_coeffs_;
-	geometry_msgs::TransformStamped transform_;
-	geometry_msgs::PoseWithCovarianceStamped pose_;
-	tf2_ros::TransformBroadcaster br_;
-	tf2_ros::Buffer tf_buffer_;
-	tf2_ros::TransformListener tf_listener_{tf_buffer_};
-	visualization_msgs::MarkerArray vis_markers;
-	std::string snap_orientation_;
-	bool has_camera_info_ = false;
-
-public:
-	virtual void onInit()
-	{
-		nh_ = getNodeHandle();
-		nh_priv_ = getPrivateNodeHandle();
-
-		image_transport::ImageTransport it_priv(nh_priv_);
-
-		// TODO: why image_transport doesn't work here?
-		img_pub_ = nh_priv_.advertise<sensor_msgs::Image>("image", 1, true);
-
-		board_ = cv::makePtr<cv::aruco::Board>();
-		board_->dictionary = cv::aruco::getPredefinedDictionary(
-			                 static_cast<cv::aruco::PREDEFINED_DICTIONARY_NAME>(nh_priv_.param("dictionary", 2)));
-		camera_matrix_ = cv::Mat::zeros(3, 3, CV_64F);
-		dist_coeffs_ = cv::Mat::zeros(8, 1, CV_64F);
-
-		std::string type, map, map_name;
-		nh_priv_.param<std::string>("type", type, "map");
-
-		if (type == "map") {
-			param(nh_priv_, "map", map);
-			loadMap(map);
-		} else if (type == "gridboard") {
-			createGridBoard();
-		} else {
-			ROS_FATAL("aruco_map: unknown type: %s", type.c_str());
-			ros::shutdown();
-		}
-
-		nh_priv_.param<std::string>("name", map_name, "map");
-		nh_priv_.param<std::string>("frame_id", transform_.child_frame_id, "aruco_map");
-		nh_priv_.param<std::string>("snap_orientation", snap_orientation_, "");
-
-		pose_pub_ = nh_priv_.advertise<geometry_msgs::PoseWithCovarianceStamped>("pose", 1);
-
-		// TODO: use synchronised subscriber
-		markers_sub_ = nh_.subscribe("markers", 1, &ArucoMap::markersCallback, this);
-		cinfo_sub = nh_.subscribe("camera_info", 1, &ArucoMap::cinfoCallback, this);
-
-		publishMapImage();
-		ROS_INFO("aruco_map: ready");
-	}
-
-	void markersCallback(const aruco_pose::MarkerArray& markers)
-	{
-		if (!has_camera_info_) return;
-		if (markers.markers.empty()) return;
-
-		int count = markers.markers.size();
-		std::vector<int> ids;
-		std::vector<std::vector<cv::Point2f>> corners;
-		ids.reserve(count);
-		corners.reserve(count);
-
-		for(auto const &marker : markers.markers) {
-			ids.push_back(marker.id);
-			std::vector<cv::Point2f> marker_corners = {
-				cv::Point2f(marker.c1.x, marker.c1.y),
-				cv::Point2f(marker.c2.x, marker.c2.y),
-				cv::Point2f(marker.c3.x, marker.c3.y),
-				cv::Point2f(marker.c4.x, marker.c4.y)
-			};
-			corners.push_back(marker_corners);
-		}
-
-		Mat obj_points, img_points;
-		cv::Vec3d rvec, tvec;
-
-		if (snap_orientation_.empty()) {
-			// simple estimation
-			int valid = cv::aruco::estimatePoseBoard(corners, ids, board_, camera_matrix_, dist_coeffs_,
-		                                             rvec, tvec, false);
-			if (!valid) return;
-
-			transform_.header.stamp = markers.header.stamp;
-			transform_.header.frame_id = markers.header.frame_id;
-			pose_.header = transform_.header;
-			fillPose(pose_.pose.pose, rvec, tvec);
-			fillTransform(transform_.transform, rvec, tvec);
-
-		} else {
-			// estimation with "snapping"
-			cv::aruco::getBoardObjectAndImagePoints(board_, corners, ids, obj_points, img_points);
-			if (obj_points.empty()) return;
-
-			double center_x = 0, center_y = 0;
-			alignObjPointsToCenter(obj_points, center_x, center_y);
-
-			int res = solvePnP(obj_points, img_points, camera_matrix_, dist_coeffs_, rvec, tvec, false);
-			if (!res) return;
-
-			fillTransform(transform_.transform, rvec, tvec);
-			try {
-				geometry_msgs::TransformStamped snap_to = tf_buffer_.lookupTransform(markers.header.frame_id,
-				                                          snap_orientation_, markers.header.stamp, ros::Duration(0.02));
-				snapOrientation(transform_.transform.rotation, snap_to.transform.rotation);
-			} catch (const tf2::TransformException& e) {
-				ROS_WARN_THROTTLE(1, "aruco_map: can't snap: %s", e.what());
-			}
-
-			geometry_msgs::TransformStamped shift;
-			shift.transform.translation.x = -center_x;
-			shift.transform.translation.y = -center_y;
-			shift.transform.rotation.w = 1;
-			tf2::doTransform(shift, transform_, transform_);
-
-			transform_.header.stamp = markers.header.stamp;
-			transform_.header.frame_id = markers.header.frame_id;
-			pose_.header = transform_.header;
-			transformToPose(transform_.transform, pose_.pose.pose);
-		}
-
-		br_.sendTransform(transform_);
-		pose_pub_.publish(pose_);
-	}
-
-	void cinfoCallback(const sensor_msgs::CameraInfoConstPtr& cinfo)
-	{
-		parseCameraInfo(cinfo, camera_matrix_, dist_coeffs_);
-		has_camera_info_ = true;
-	}
-
-	void alignObjPointsToCenter(Mat &obj_points, double &center_x, double &center_y) const
-	{
-		// Align object points to the center of mass
-		double sum_x = 0;
-		double sum_y = 0;
-
-		for (int i = 0; i < obj_points.rows; i++) 		{
-			sum_x += obj_points.at<float>(i, 0);
-			sum_y += obj_points.at<float>(i, 1);
-		}
-
-		center_x = sum_x / obj_points.rows;
-		center_y = sum_y / obj_points.rows;
-
-		for (int i = 0; i < obj_points.rows; i++) 		{
-			obj_points.at<float>(i, 0) -= center_x;
-			obj_points.at<float>(i, 1) -= center_y;
-		}
-	}
-
-	void loadMap(std::string filename)
-	{
-		std::ifstream f(filename);
-		std::string line;
-
-		if (!f.good()) {
-			ROS_FATAL("aruco_map: %s - %s", strerror(errno), filename.c_str());
-			ros::shutdown();
-		}
-
-		while (std::getline(f, line)) {
-			int id;
-			double length, x, y, z, yaw, pitch, roll;
-
-			std::istringstream s(line);
-			ROS_INFO("aruco_map: parse line: %s", line.c_str());
-
-			if (!(s >> id >> length >> x >> y >> z >> yaw >> pitch >> roll)) {
-				ROS_ERROR("aruco_map: cannot parse line: %s", line.c_str());
-				continue;
-			}
-			addMarker(id, length, x, y, z, yaw, pitch, roll);
-		}
-
-		ROS_INFO("aruco_map: loading %s complete (%d markers)", filename.c_str(), static_cast<int>(board_->ids.size()));
-	}
-
-	void createGridBoard()
-	{
-		ROS_INFO("aruco_map: generate gridboard");
-		ROS_WARN("aruco_map: gridboard maps are deprecated");
-
-		int markers_x, markers_y, first_marker;
-		double markers_side, markers_sep_x, markers_sep_y;
-		std::vector<int> marker_ids;
-		nh_priv_.param<int>("markers_x", markers_x, 10);
-		nh_priv_.param<int>("markers_y", markers_y, 10);
-		nh_priv_.param<int>("first_marker", first_marker, 0);
-
-		param(nh_priv_, "markers_side", markers_side);
-		param(nh_priv_, "markers_sep_x", markers_sep_x);
-		param(nh_priv_, "markers_sep_y", markers_sep_y);
-
-		if (nh_priv_.getParam("marker_ids", marker_ids)) {
-			if ((unsigned int)(markers_x * markers_y) != marker_ids.size()) {
-				ROS_FATAL("~marker_ids length should be equal to ~markers_x * ~markers_y");
-				exit(1);
-			}
-		} else {
-			// Fill marker_ids automatically
-			marker_ids.resize(markers_x * markers_y);
-			for (int i = 0; i < markers_x * markers_y; i++)
-			{
-				marker_ids.at(i) = first_marker++;
-			}
-		}
-
-		createCustomGridBoard(board_, markers_x, markers_y, markers_side, markers_sep_x, markers_sep_y, marker_ids);
-	}
-
-	void addMarker(int id, double length, double x, double y, double z,
-				   double yaw, double pitch, double roll)
-	{
-		// Create transform
-		geometry_msgs::TransformStamped t;
-		t.transform.translation.x = x;
-		t.transform.translation.y = y;
-		t.transform.translation.z = z;
-		tf::Quaternion q;
-		q.setRPY(roll, pitch, yaw);
-		tf::quaternionTFToMsg(q, t.transform.rotation);
-
-		// TODO: process roll pitch yaw
-		vector<cv::Point3f> obj_points(4);
-		setMarkerObjectPoints(x, y, z, yaw, length, obj_points);
-		board_->ids.push_back(id);
-		board_->objPoints.push_back(obj_points);
-	}
-
-	void setMarkerObjectPoints(float x, float y, float z, float yaw, float length,
-	                           vector<cv::Point3f>& obj_points)
-	{
-		obj_points[0] = cv::Point3f(x - length / 2, y + length / 2, 0);
-		obj_points[1] = obj_points[0] + cv::Point3f(length, 0, 0);
-		obj_points[2] = obj_points[0] + cv::Point3f(length, -length, 0);
-		obj_points[3] = obj_points[0] + cv::Point3f(0, -length, 0);
-	}
-
-	void publishMapImage()
-	{
-		cv::Mat image;
-		cv_bridge::CvImage msg;
-		cv::aruco::drawPlanarBoard(board_, cv::Size(2000, 2000), image, 50, 1);
-		cv::cvtColor(image, image, CV_GRAY2BGR);
-		msg.encoding = sensor_msgs::image_encodings::BGR8;
-		msg.image = image;
-		img_pub_.publish(msg.toImageMsg());
-	}
-};
-
-PLUGINLIB_EXPORT_CLASS(ArucoMap, nodelet::Nodelet)

aruco_pose/src/aruco_pose.cpp

@@ -0,0 +1,350 @@
+#include <algorithm>
+#include <nodelet/nodelet.h>
+#include <image_transport/image_transport.h>
+#include <cv_bridge/cv_bridge.h>
+#include <opencv2/imgproc/imgproc.hpp>
+#include <pluginlib/class_list_macros.h>
+#include <geometry_msgs/TransformStamped.h>
+#include <geometry_msgs/PoseStamped.h>
+#include <geometry_msgs/PoseWithCovarianceStamped.h>
+#include <visualization_msgs/MarkerArray.h>
+#include <tf/transform_datatypes.h>
+#include <opencv2/calib3d/calib3d.hpp>
+#include <opencv2/opencv.hpp>
+#include <opencv2/aruco.hpp>
+#include <opencv2/aruco/dictionary.hpp>
+#include <stdio.h>
+#include <tf/transform_broadcaster.h>
+
+#include "util.h"
+
+using std::vector;
+using std::string;
+
+namespace aruco_pose {
+
+class ArucoPose : public nodelet::Nodelet {
+    tf::TransformBroadcaster br;
+    cv::Ptr<cv::aruco::Dictionary> dictionary;
+    cv::Ptr<cv::aruco::DetectorParameters> parameters;
+    cv::Ptr<cv::aruco::Board> board;
+    std::string frame_id_;
+    image_transport::CameraSubscriber img_sub;
+    image_transport::Publisher img_pub;
+    ros::Publisher marker_pub;
+    ros::Publisher pose_pub;
+    ros::NodeHandle nh_, nh_priv_;
+
+    virtual void onInit();
+    void createBoard();
+    cv::Point3f getObjPointsCenter(cv::Mat objPoints);
+    void detect(const sensor_msgs::ImageConstPtr&, const sensor_msgs::CameraInfoConstPtr&);
+    void parseCameraInfo(const sensor_msgs::CameraInfoConstPtr&, cv::Mat&, cv::Mat&);
+    tf::Transform aruco2tf(cv::Mat rvec, cv::Mat tvec);
+};
+
+void ArucoPose::onInit() {
+    ROS_INFO("Initializing aruco_pose");
+    nh_ = getNodeHandle();
+    nh_priv_ = getPrivateNodeHandle();
+
+    nh_priv_.param("frame_id", frame_id_, std::string("aruco_map"));
+
+    dictionary = cv::aruco::getPredefinedDictionary(cv::aruco::DICT_4X4_1000);
+    parameters = cv::aruco::DetectorParameters::create();
+
+    try
+    {
+        createBoard();
+    }
+    catch (const std::exception &exc)
+    {
+        std::cerr << exc.what();
+        exit(0);
+    }
+
+    image_transport::ImageTransport it(nh_);
+    img_sub = it.subscribeCamera("image", 1, &ArucoPose::detect, this);
+
+    image_transport::ImageTransport it_priv(nh_priv_);
+    img_pub = it_priv.advertise("debug", 1);
+
+    pose_pub = nh_priv_.advertise<geometry_msgs::PoseStamped>("pose", 1);
+
+    ROS_INFO("aruco_pose nodelet inited");
+}
+
+cv::Ptr<cv::aruco::Board> createCustomGridBoard(int markersX, int markersY, float markerLength, float markerSeparationX, float markerSeparationY,
+                            const cv::Ptr<cv::aruco::Dictionary> &dictionary, std::vector<int> ids) {
+
+    CV_Assert(markersX > 0 && markersY > 0 && markerLength > 0 && markerSeparationX > 0 && markerSeparationY > 0);
+
+    cv::Ptr<cv::aruco::Board> res = cv::makePtr<cv::aruco::Board>();
+
+    res->dictionary = dictionary;
+
+    size_t totalMarkers = (size_t) markersX * markersY;
+    res->ids = ids;
+    res->objPoints.reserve(totalMarkers);
+
+    // calculate Board objPoints
+    float maxY = (float)markersY * markerLength + (markersY - 1) * markerSeparationY;
+    for(int y = 0; y < markersY; y++) {
+        for(int x = 0; x < markersX; x++) {
+            std::vector< cv::Point3f > corners;
+            corners.resize(4);
+            corners[0] = cv::Point3f(x * (markerLength + markerSeparationX),
+                                 maxY - y * (markerLength + markerSeparationY), 0);
+            corners[1] = corners[0] + cv::Point3f(markerLength, 0, 0);
+            corners[2] = corners[0] + cv::Point3f(markerLength, -markerLength, 0);
+            corners[3] = corners[0] + cv::Point3f(0, -markerLength, 0);
+            res->objPoints.push_back(corners);
+        }
+    }
+
+    return res;
+}
+
+cv::Ptr<cv::aruco::Board> createCustomBoard(std::map<string, string> markers, const cv::Ptr<cv::aruco::Dictionary> &dictionary) {
+    cv::Ptr<cv::aruco::Board> res = cv::makePtr<cv::aruco::Board>();
+
+    res->dictionary = dictionary;
+
+    size_t total_markers = markers.size();
+    res->ids.reserve(total_markers);
+    res->objPoints.reserve(total_markers);
+
+    // Generate ids and objPoints
+    for(auto const &marker : markers) {
+        res->ids.push_back(std::stoi(marker.first));
+
+        vector<string> parts;
+        parts = strSplit(marker.second, " ");
+
+        float size = std::stof(parts.at(0));
+        float x = std::stof(parts.at(1));
+        float y = std::stof(parts.at(2));
+        float z = std::stof(parts.at(3));
+        float yaw = std::stof(parts.at(4));
+        float pitch = std::stof(parts.at(5));
+        float roll = std::stof(parts.at(6));
+
+        vector<cv::Point3f> corners;
+        corners.resize(4);
+        corners[0] = cv::Point3f(x - size / 2, y + size / 2, 0);
+        corners[1] = corners[0] + cv::Point3f(size, 0, 0);
+        corners[2] = corners[0] + cv::Point3f(size, -size, 0);
+        corners[3] = corners[0] + cv::Point3f(0, -size, 0);
+
+        // TODO: process yaw, pitch, roll
+
+        res->objPoints.push_back(corners);
+    }
+
+    return res;
+}
+
+#include "fix.cpp"
+
+void ArucoPose::createBoard()
+{
+    static auto map_image_pub = nh_priv_.advertise<sensor_msgs::Image>("map_image", 1, true);
+    cv_bridge::CvImage map_image_msg;
+    cv::Mat map_image;
+
+    std::string type;
+
+    nh_priv_.param<std::string>("type", type, "gridboard");
+    if (type == "gridboard")
+    {
+        ROS_INFO("Initialize gridboard");
+
+        int markers_x, markers_y, first_marker;
+        float markers_side, markers_sep_x, markers_sep_y;
+        std::vector<int> marker_ids;
+        nh_priv_.param<int>("markers_x", markers_x, 10);
+        nh_priv_.param<int>("markers_y", markers_y, 10);
+        nh_priv_.param<int>("first_marker", first_marker, 0);
+
+        if (!nh_priv_.getParam("markers_side", markers_side))
+        {
+            ROS_ERROR("gridboard: required parameter ~markers_side is not set.");
+            exit(1);
+        }
+
+        if (!nh_priv_.getParam("markers_sep_x", markers_sep_x))
+        {
+            if (!nh_priv_.getParam("markers_sep", markers_sep_x))
+            {
+               ROS_ERROR("gridboard: ~markers_sep_x or ~markers_sep parameters are required");
+               exit(1);
+            }
+        }
+
+        if (!nh_priv_.getParam("markers_sep_y", markers_sep_y))
+        {
+            if (!nh_priv_.getParam("markers_sep", markers_sep_y))
+            {
+               ROS_ERROR("gridboard: ~markers_sep_y or ~markers_sep parameters are required");
+               exit(1);
+            }
+        }
+
+        if (nh_priv_.getParam("marker_ids", marker_ids))
+        {
+            if (markers_x * markers_y != marker_ids.size())
+            {
+                ROS_FATAL("~marker_ids length should be equal to ~markers_x * ~markers_y");
+                exit(1);
+            }
+        }
+        else
+        {
+            // Fill marker_ids automatically
+            marker_ids.resize(markers_x * markers_y);
+            for(int i = 0; i < markers_x * markers_y; i++)
+            {
+                marker_ids.at(i) = first_marker++;
+            }
+        }
+
+        // Create grid board
+        board = createCustomGridBoard(markers_x, markers_y, markers_side, markers_sep_x, markers_sep_y, dictionary, marker_ids);
+
+        // Publish map image for debugging
+        _drawPlanarBoard(board,  cv::Size(2000, 2000), map_image, 50, 1);
+
+        cv::cvtColor(map_image, map_image, CV_GRAY2BGR);
+
+        map_image_msg.encoding = sensor_msgs::image_encodings::BGR8;
+        map_image_msg.image = map_image;
+        map_image_pub.publish(map_image_msg.toImageMsg());
+    }
+    else if (type == "custom")
+    {
+        ROS_INFO("Initialize a custom board");
+
+        std::map<string, string> markers;
+        nh_priv_.getParam("markers", markers);
+
+        board = createCustomBoard(markers, dictionary);
+
+        ROS_INFO("Draw a custom board");
+        // Publish map image for debugging
+        _drawPlanarBoard(board,  cv::Size(2000, 2000), map_image, 50, 1);
+
+        cv::cvtColor(map_image, map_image, CV_GRAY2BGR);
+
+        map_image_msg.encoding = sensor_msgs::image_encodings::BGR8;
+        map_image_msg.image = map_image;
+        map_image_pub.publish(map_image_msg.toImageMsg());
+    }
+    else
+    {
+        ROS_ERROR("Incorrect map type '%s'", type.c_str());
+    }
+}
+
+cv::Point3f ArucoPose::getObjPointsCenter(cv::Mat objPoints) {
+    float min_x = std::numeric_limits<float>::max();
+    float max_x = std::numeric_limits<float>::min();
+    float min_y = min_x, max_y = max_x;
+    for (int i = 0; i < objPoints.rows; i++) {
+        max_x = std::max(max_x, objPoints.at<float>(i, 0));
+        max_y = std::max(max_y, objPoints.at<float>(i, 1));
+        min_x = std::min(min_x, objPoints.at<float>(i, 0));
+        min_y = std::min(min_y, objPoints.at<float>(i, 1));
+    }
+    cv::Point3f res((min_x + max_x) / 2, (min_y + max_y) / 2, 0);
+    return res;
+}
+
+void ArucoPose::detect(const sensor_msgs::ImageConstPtr& msg, const sensor_msgs::CameraInfoConstPtr &cinfo) {
+    cv::Mat image = cv_bridge::toCvShare(msg, "bgr8")->image;
+
+    std::vector<int> markerIds;
+    std::vector<std::vector<cv::Point2f>> markerCorners;
+    std::vector<std::vector<cv::Point2f>> rejectedCandidates;
+
+    cv::aruco::detectMarkers(image, dictionary, markerCorners, markerIds, parameters, rejectedCandidates);
+
+    cv::Mat cameraMatrix(3, 3, CV_64F);
+    cv::Mat distCoeffs(8, 1, CV_64F);
+    parseCameraInfo(cinfo, cameraMatrix, distCoeffs);
+
+    int valid = 0;
+    cv::Mat rvec, tvec, objPoints;
+
+    if (markerIds.size() > 0) {
+
+        valid = _estimatePoseBoard(markerCorners, markerIds, board, cameraMatrix, distCoeffs,
+                                       rvec, tvec, false, objPoints);
+
+        if (valid) {
+            // Send map transform
+            tf::StampedTransform transform(aruco2tf(rvec, tvec), msg->header.stamp, cinfo->header.frame_id, frame_id_);
+            br.sendTransform(transform);
+
+            // Publish map pose
+            static geometry_msgs::PoseStamped ps;
+            ps.header.frame_id = frame_id_;
+            ps.header.stamp = msg->header.stamp;
+            ps.pose.orientation.w = 1;
+            pose_pub.publish(ps);
+
+            // Send reference point
+            cv::Point3f ref = getObjPointsCenter(objPoints);
+            tf::Vector3 ref_vector3 = tf::Vector3(ref.x, ref.y, ref.z);
+            tf::Quaternion q(0, 0, 0);
+            static tf::StampedTransform ref_transform;
+            ref_transform.stamp_ = msg->header.stamp;
+            ref_transform.frame_id_ = frame_id_;
+            ref_transform.child_frame_id_ = "aruco_map_reference";
+            ref_transform.setOrigin(ref_vector3);
+            ref_transform.setRotation(q);
+            br.sendTransform(ref_transform);
+        }
+    }
+
+    if (img_pub.getNumSubscribers() > 0)
+    {
+        cv::aruco::drawDetectedMarkers(image, markerCorners, markerIds); // draw markers
+        if (valid)
+        {
+            cv::aruco::drawAxis(image, cameraMatrix, distCoeffs, rvec, tvec, 0.3); // draw board axis
+        }
+        cv_bridge::CvImage out_msg;
+        out_msg.header.frame_id = msg->header.frame_id;
+        out_msg.header.stamp = msg->header.stamp;
+        out_msg.encoding = sensor_msgs::image_encodings::BGR8;
+        out_msg.image = image;
+        img_pub.publish(out_msg.toImageMsg());
+    }
+}
+
+void ArucoPose::parseCameraInfo(const sensor_msgs::CameraInfoConstPtr &cinfo, cv::Mat &cameraMat, cv::Mat &distCoeffs) {
+    for (int i = 0; i < 3; ++i) {
+        for (int j = 0; j < 3; ++j) {
+            cameraMat.at<double>(i, j) = cinfo->K[3 * i + j];
+        }
+    }
+    for (int k = 0; k < cinfo->D.size(); k++) {
+        distCoeffs.at<double>(k) = cinfo->D[k];
+    }
+}
+
+tf::Transform ArucoPose::aruco2tf(cv::Mat rvec, cv::Mat tvec) {
+
+    cv::Mat rot;
+    cv::Rodrigues(rvec, rot);
+
+    tf::Matrix3x3 tf_rot(rot.at<double>(0,0), rot.at<double>(0,1), rot.at<double>(0,2),
+                         rot.at<double>(1,0), rot.at<double>(1,1), rot.at<double>(1,2),
+                         rot.at<double>(2,0), rot.at<double>(2,1), rot.at<double>(2,2));
+    tf::Vector3 tf_orig(tvec.at<double>(0,0), tvec.at<double>(1,0), tvec.at<double>(2,0));
+    return tf::Transform(tf_rot, tf_orig);
+}
+
+PLUGINLIB_EXPORT_CLASS(ArucoPose, nodelet::Nodelet)
+
+}

aruco_pose/src/fix.cpp

@@ -0,0 +1,145 @@
+using namespace cv;
+using namespace cv::aruco;
+
+// Temporal fix!
+// TODO: remove
+// fix strange bug in our OpenCV version
+
+void _getBoardObjectAndImagePoints(const Ptr<aruco::Board> &board, InputArrayOfArrays detectedCorners,
+    InputArray detectedIds, OutputArray objPoints, OutputArray imgPoints) {
+
+    CV_Assert(board->ids.size() == board->objPoints.size());
+    CV_Assert(detectedIds.total() == detectedCorners.total());
+
+    size_t nDetectedMarkers = detectedIds.total();
+
+    std::vector< Point3f > objPnts;
+    objPnts.reserve(nDetectedMarkers);
+
+    std::vector< Point2f > imgPnts;
+    imgPnts.reserve(nDetectedMarkers);
+
+    // look for detected markers that belong to the board and get their information
+    for(unsigned int i = 0; i < nDetectedMarkers; i++) {
+        int currentId = detectedIds.getMat().ptr< int >(0)[i];
+        for(unsigned int j = 0; j < board->ids.size(); j++) {
+            if(currentId == board->ids[j]) {
+                for(int p = 0; p < 4; p++) {
+                    objPnts.push_back(board->objPoints[j][p]);
+                    imgPnts.push_back(detectedCorners.getMat(i).ptr< Point2f >(0)[p]);
+                }
+            }
+        }
+    }
+
+    // create output
+    Mat(objPnts).copyTo(objPoints);
+    Mat(imgPnts).copyTo(imgPoints);
+}
+
+int _estimatePoseBoard(InputArrayOfArrays _corners, InputArray _ids, const Ptr<aruco::Board> &board,
+                      InputArray _cameraMatrix, InputArray _distCoeffs, OutputArray _rvec,
+                      OutputArray _tvec, bool useExtrinsicGuess, Mat &objPoints) {
+
+    CV_Assert(_corners.total() == _ids.total());
+
+    // get object and image points for the solvePnP function
+    Mat /*objPoints, */imgPoints;
+    _getBoardObjectAndImagePoints(board, _corners, _ids, objPoints, imgPoints);
+
+    CV_Assert(imgPoints.total() == objPoints.total());
+
+    if(objPoints.total() == 0) // 0 of the detected markers in board
+        return 0;
+
+//    std::cout << "objPoints: " << objPoints << std::endl;
+//    std::cout << "imgPoints: " << imgPoints << std::endl;
+
+    solvePnP(objPoints, imgPoints, _cameraMatrix, _distCoeffs, _rvec, _tvec, useExtrinsicGuess);
+
+    // divide by four since all the four corners are concatenated in the array for each marker
+    return (int)objPoints.total() / 4;
+}
+
+void _drawPlanarBoard(Board *_board, Size outSize, OutputArray _img, int marginSize,
+                     int borderBits) {
+
+    CV_Assert(outSize.area() > 0);
+    CV_Assert(marginSize >= 0);
+
+    _img.create(outSize, CV_8UC1);
+    Mat out = _img.getMat();
+    out.setTo(Scalar::all(255));
+    out.adjustROI(-marginSize, -marginSize, -marginSize, -marginSize);
+
+    // calculate max and min values in XY plane
+    CV_Assert(_board->objPoints.size() > 0);
+    float minX, maxX, minY, maxY;
+    minX = maxX = _board->objPoints[0][0].x;
+    minY = maxY = _board->objPoints[0][0].y;
+
+    for(unsigned int i = 0; i < _board->objPoints.size(); i++) {
+        for(int j = 0; j < 4; j++) {
+            minX = min(minX, _board->objPoints[i][j].x);
+            maxX = max(maxX, _board->objPoints[i][j].x);
+            minY = min(minY, _board->objPoints[i][j].y);
+            maxY = max(maxY, _board->objPoints[i][j].y);
+        }
+    }
+
+    float sizeX = maxX - minX;
+    float sizeY = maxY - minY;
+
+    // proportion transformations
+    float xReduction = sizeX / float(out.cols);
+    float yReduction = sizeY / float(out.rows);
+
+    // determine the zone where the markers are placed
+    if(xReduction > yReduction) {
+        int nRows = int(sizeY / xReduction);
+        int rowsMargins = (out.rows - nRows) / 2;
+        out.adjustROI(-rowsMargins, -rowsMargins, 0, 0);
+    } else {
+        int nCols = int(sizeX / yReduction);
+        int colsMargins = (out.cols - nCols) / 2;
+        out.adjustROI(0, 0, -colsMargins, -colsMargins);
+    }
+
+    // now paint each marker
+    Dictionary &dictionary = *(_board->dictionary);
+    Mat marker;
+    Point2f outCorners[3];
+    Point2f inCorners[3];
+    for(unsigned int m = 0; m < _board->objPoints.size(); m++) {
+        // transform corners to markerZone coordinates
+        for(int j = 0; j < 3; j++) {
+            Point2f pf = Point2f(_board->objPoints[m][j].x, _board->objPoints[m][j].y);
+            // move top left to 0, 0
+            pf -= Point2f(minX, minY);
+            pf.x = pf.x / sizeX * float(out.cols);
+            pf.y = (1.0f - pf.y / sizeY) * float(out.rows);
+            outCorners[j] = pf;
+        }
+
+        // get marker
+        Size dst_sz(outCorners[2] - outCorners[0]); // assuming CCW order
+        dst_sz.width = dst_sz.height = std::min(dst_sz.width, dst_sz.height); //marker should be square
+        dictionary.drawMarker(_board->ids[m], dst_sz.width, marker, borderBits);
+
+        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);
+
+        // remove perspective
+        Mat transformation = getAffineTransform(inCorners, outCorners);
+        warpAffine(marker, out, transformation, out.size(), INTER_LINEAR,
+                        BORDER_TRANSPARENT);
+    }
+}

aruco_pose/src/gridboard.h

@@ -1,22 +0,0 @@
-void createCustomGridBoard(cv::Ptr<cv::aruco::Board>& board, int markersX, int markersY, float markerLength,
-                           float markerSeparationX, float markerSeparationY, std::vector<int> ids)
-{
-	size_t totalMarkers = (size_t) markersX * markersY;
-	board->ids = ids;
-	board->objPoints.reserve(totalMarkers);
-
-	// calculate Board objPoints
-	float maxY = (float)markersY * markerLength + (markersY - 1) * markerSeparationY;
-	for(int y = 0; y < markersY; y++) {
-		for(int x = 0; x < markersX; x++) {
-			std::vector< cv::Point3f > corners;
-			corners.resize(4);
-			corners[0] = cv::Point3f(x * (markerLength + markerSeparationX),
-								 maxY - y * (markerLength + markerSeparationY), 0);
-			corners[1] = corners[0] + cv::Point3f(markerLength, 0, 0);
-			corners[2] = corners[0] + cv::Point3f(markerLength, -markerLength, 0);
-			corners[3] = corners[0] + cv::Point3f(0, -markerLength, 0);
-			board->objPoints.push_back(corners);
-		}
-	}
-}

aruco_pose/src/util.h

@@ -0,0 +1,20 @@
+#pragma once
+
+#include <vector>
+#include <string>
+
+std::vector<std::string> strSplit(const std::string& str, const std::string& delim)
+{
+    std::vector<std::string> tokens;
+    size_t prev = 0, pos = 0;
+    do
+    {
+        pos = str.find(delim, prev);
+        if (pos == std::string::npos) pos = str.length();
+        std::string token = str.substr(prev, pos-prev);
+        if (!token.empty()) tokens.push_back(token);
+        prev = pos + delim.length();
+    }
+    while (pos < str.length() && prev < str.length());
+    return tokens;
+}

aruco_pose/src/utils.h

@@ -1,109 +0,0 @@
-#pragma once
-
-#include <ros/ros.h>
-#include <tf/transform_datatypes.h>
-#include <geometry_msgs/Quaternion.h>
-#include <geometry_msgs/Pose.h>
-#include <geometry_msgs/Vector3.h>
-#include <sensor_msgs/CameraInfo.h>
-#include <opencv2/opencv.hpp>
-
-// Read required param or shutdown the node
-template<typename T>
-static void param(ros::NodeHandle nh, const std::string& param_name, T& param_val)
-{
-	if (!nh.getParam(param_name, param_val)) {
-		ROS_FATAL("Required param %s is not defined", param_name.c_str());
-		ros::shutdown();
-	}
-}
-
-static void parseCameraInfo(const sensor_msgs::CameraInfoConstPtr& cinfo,
-                     cv::Mat& matrix, cv::Mat& dist)
-{
-	for (unsigned int i = 0; i < 3; ++i)
-		for (unsigned int j = 0; j < 3; ++j)
-			matrix.at<double>(i, j) = cinfo->K[3 * i + j];
-
-	for (unsigned int k = 0; k < cinfo->D.size(); k++)
-		dist.at<double>(k) = cinfo->D[k];
-}
-
-inline void rotatePoint(cv::Point3f& p, cv::Point3f origin, float angle)
-{
-	float s = sin(angle);
-	float c = cos(angle);
-
-	// translate point back to origin:
-	p.x -= origin.x;
-	p.y -= origin.y;
-
-	// rotate point
-	float xnew = p.x * c - p.y * s;
-	float ynew = p.x * s + p.y * c;
-
-	// translate point back:
-	p.x = xnew + origin.x;
-	p.y = ynew + origin.y;
-}
-
-inline void fillPose(geometry_msgs::Pose& pose, const cv::Vec3d& rvec, const cv::Vec3d& tvec)
-{
-	pose.position.x = tvec[0];
-	pose.position.y = tvec[1];
-	pose.position.z = tvec[2];
-
-	double angle = norm(rvec);
-	cv::Vec3d axis = rvec / angle;
-
-	tf2::Quaternion q;
-	q.setRotation(tf2::Vector3(axis[0], axis[1], axis[2]), angle);
-
-	pose.orientation.w = q.w();
-	pose.orientation.x = q.x();
-	pose.orientation.y = q.y();
-	pose.orientation.z = q.z();
-}
-
-inline void fillTransform(geometry_msgs::Transform& transform, const cv::Vec3d& rvec, const cv::Vec3d& tvec)
-{
-	transform.translation.x = tvec[0];
-	transform.translation.y = tvec[1];
-	transform.translation.z = tvec[2];
-
-	double angle = norm(rvec);
-	cv::Vec3d axis = rvec / angle;
-
-	tf2::Quaternion q;
-	q.setRotation(tf2::Vector3(axis[0], axis[1], axis[2]), angle);
-
-	transform.rotation.w = q.w();
-	transform.rotation.x = q.x();
-	transform.rotation.y = q.y();
-	transform.rotation.z = q.z();
-}
-
-inline void fillTranslation(geometry_msgs::Vector3& translation, const cv::Vec3d& tvec)
-{
-	translation.x = tvec[0];
-	translation.y = tvec[1];
-	translation.z = tvec[2];
-}
-
-inline void snapOrientation(geometry_msgs::Quaternion& to, const geometry_msgs::Quaternion& from)
-{
-	tf::Quaternion q;
-	q.setRPY(0, 0, -tf::getYaw(to) + tf::getYaw(from));
-	tf::Quaternion pq;
-	tf::quaternionMsgToTF(from, pq);
-	pq = pq * q;
-	tf::quaternionTFToMsg(pq, to);
-}
-
-inline void transformToPose(const geometry_msgs::Transform& transform, geometry_msgs::Pose& pose)
-{
-	pose.position.x = transform.translation.x;
-	pose.position.y = transform.translation.y;
-	pose.position.z = transform.translation.z;
-	pose.orientation = transform.rotation;
-}

book.json

@@ -4,19 +4,21 @@
     "author": "Copter Express",
     "language": "ru",
     "root": "docs/",
-    "plugins": ["youtube", "richquotes", "versions", "yametrika"],
+    "plugins": [
+        "youtube",
+        "richquotes@https://github.com/okalachev/gitbook-plugin-richquotes.git",
+        "yametrika",
+        "anchors",
+        "validate-links",
+        "bulk-redirect@https://github.com/okalachev/gitbook-plugin-bulk-redirect.git"
+    ],
     "pluginsConfig": {
-        "disqus": {
-            "shortName": "coex-clever"
-        },
-        "versions": {
-            "type": "tags"
-        },
         "yametrika": {
             "id": 49359238
+        },
+        "bulk-redirect": {
+            "basepath": "/",
+            "redirectsFile": "redirects.json"
         }
-    },
-    "structure": {
-        "glossary": "_GLOSSARY.md"
     }
 }

builder/assets/index.html

@@ -4,6 +4,7 @@
 	<!-- <li><a href="">View user reference</a> (<a href="http://clever.copterexpress.com">http://clever.copterexpress.com</a> snapshot)</li> -->
 	<li><a href="" id="wvs">View image topics</a> (<code>web_video_server</code>)</li>
 	<li><a href="" id="butterfly">Open web terminal</a> (<code>Butterfly</code>)</li>
+	<li><a href="/docs">Documentation</a> (<code>gitbook</code>)</li>
 	<!-- <li><a href="viz.html">View 3D visualization</a> (<code>ros3djs</code>)</li> -->
 </ul>
 

builder/image-build.sh

@@ -84,6 +84,15 @@ ${BUILDER_DIR}/image-chroot.sh ${IMAGE_PATH} exec ${SCRIPTS_DIR}'/image-init.sh'
 ${BUILDER_DIR}/image-chroot.sh ${IMAGE_PATH} copy ${SCRIPTS_DIR}'/assets/monkey-clever' '/root/'
 ${BUILDER_DIR}/image-chroot.sh ${IMAGE_PATH} copy ${SCRIPTS_DIR}'/assets/index.html' '/usr/share/monkey-static/'
 
+# Gitbook
+apt-get install -y curl gnupg
+curl -sL https://deb.nodesource.com/setup_11.x | bash -
+apt-get install -y nodejs
+npm install gitbook-cli -g
+gitbook build ${REPO_DIR}'/docs'
+${BUILDER_DIR}/image-chroot.sh ${IMAGE_PATH} copy ${REPO_DIR}'/docs/_book/' '/usr/share/monkey-static/docs/'
+
+
 # Butterfly
 ${BUILDER_DIR}/image-chroot.sh ${IMAGE_PATH} copy ${SCRIPTS_DIR}'/assets/butterfly.service' '/lib/systemd/system/'
 ${BUILDER_DIR}/image-chroot.sh ${IMAGE_PATH} copy ${SCRIPTS_DIR}'/assets/butterfly.socket' '/lib/systemd/system/'

builder/image-software.sh

@@ -58,7 +58,7 @@ echo_stamp "Install apt keys & repos"
 # TODO: This STDOUT consist 'OK'
 curl http://repo.coex.space/aptly_repo_signing.key 2> /dev/null | apt-key add -
 apt-get update \
-&& apt-get install --no-install-recommends -y -qq dirmngr=2.1.18-8~deb9u2 > /dev/null \
+&& apt-get install --no-install-recommends -y -qq dirmngr=2.1.18-8~deb9u3 > /dev/null \
 && apt-key adv --keyserver hkp://keyserver.ubuntu.com:80 --recv-key 421C365BD9FF1F717815A3895523BAEEB01FA116
 
 echo "deb http://packages.ros.org/ros/ubuntu stretch main" > /etc/apt/sources.list.d/ros-latest.list
@@ -92,13 +92,14 @@ libjpeg8-dev=8d1-2 \
 tcpdump \
 ltrace \
 libpoco-dev=1.7.6+dfsg1-5+deb9u1 \
-python-rosdep=0.12.2-1 \
+python-rosdep=0.13.0-1 \
 python-rosinstall-generator=0.1.14-1 \
 python-wstool=0.1.17-1 \
 python-rosinstall=0.7.8-1 \
 build-essential=12.3 \
 libffi-dev \
 monkey=1.6.9-1 \
+pigpio python-pigpio python3-pigpio \
 && echo_stamp "Everything was installed!" "SUCCESS" \
 || (echo_stamp "Some packages wasn't installed!" "ERROR"; exit 1)
 
@@ -106,15 +107,24 @@ monkey=1.6.9-1 \
 sed -i "s/updates_available//" /usr/share/byobu/status/status
 # sed -i "s/updates_available//" /home/pi/.byobu/status
 
-echo_stamp "Upgrade pip"
-my_travis_retry pip install --upgrade pip
-my_travis_retry pip3 install --upgrade pip3
+#echo_stamp "Upgrade pip"
+#my_travis_retry pip install --upgrade pip
+#my_travis_retry pip3 install --upgrade pip
+
+echo_stamp "Not upgrading system pip due to https://github.com/pypa/pip/issues/5599"
+
+echo_stamp "Make sure both pip and pip3 are installed"
+pip --version
+pip3 --version
 
 echo_stamp "Install and enable Butterfly (web terminal)"
 my_travis_retry pip3 install butterfly
 my_travis_retry pip3 install butterfly[systemd]
 systemctl enable butterfly.socket
 
+echo_stamp "Install ws281x library"
+my_travis_retry pip install rpi_ws281x
+
 echo_stamp "Setup Monkey"
 mv /etc/monkey/sites/default /etc/monkey/sites/default.orig
 mv /root/monkey-clever /etc/monkey/sites/default

clever/CMakeLists.txt

@@ -167,16 +167,12 @@ add_executable(camera_markers src/camera_markers.cpp)
 
 add_executable(frames src/frames.cpp)
 
-add_executable(vpe_publisher src/vpe_publisher.cpp)
-
 target_link_libraries(rc ${catkin_LIBRARIES})
 
 target_link_libraries(camera_markers ${catkin_LIBRARIES})
 
 target_link_libraries(frames ${catkin_LIBRARIES})
 
-target_link_libraries(vpe_publisher ${catkin_LIBRARIES})
-
 ## Rename C++ executable without prefix
 ## The above recommended prefix causes long target names, the following renames the
 ## target back to the shorter version for ease of user use

clever/launch/aruco.launch

@@ -1,35 +1,24 @@
 <launch>
-    <arg name="aruco_detect" default="true"/>
-    <arg name="aruco_map" default="false"/>
-    <arg name="aruco_vpe" default="false"/>
+    <remap from="image" to="main_camera/image_raw"/>
+    <remap from="camera_info" to="main_camera/camera_info"/>
 
-    <!-- detect aruco markers, estimate poses -->
-    <node pkg="nodelet" if="$(arg aruco_detect)" clear_params="true" type="nodelet" name="aruco_detect" args="load aruco_pose/aruco_detect nodelet_manager" output="screen">
-        <remap from="image_raw" to="main_camera/image_raw"/>
-        <remap from="camera_info" to="main_camera/camera_info"/>
+    <node pkg="nodelet" type="nodelet" name="aruco_pose" args="load aruco_pose/aruco_pose nodelet_manager" clear_params="true">
+        <param name="frame_id" value="aruco_map_raw"/>
+        <param name="type" value="gridboard"/>
+        <param name="markers_x" value="1"/>
+        <param name="markers_y" value="6"/>
+        <param name="first_marker" value="240"/>
+        <param name="markers_side" value="0.3362"/>
+        <param name="markers_sep" value="0.46"/>
 
-        <param name="length" value="0.32"/>
-        <param name="snap_orientation" value="local_origin"/>
-        <param name="estimate_poses" value="true"/>
+        <!-- Custom gridboard: -->
+        <!--<rosparam param="marker_ids">[6, 5, 4, 3, 2, 1]</rosparam>-->
     </node>
 
-    <!-- estimate aruco map pose -->
-    <node pkg="nodelet" clear_params="true" type="nodelet" name="aruco_map" if="$(arg aruco_map)" args="load aruco_pose/aruco_map nodelet_manager" output="screen">
-        <remap from="markers" to="aruco_detect/markers"/>
-        <remap from="camera_info" to="main_camera/camera_info"/>
-        <param name="type" value="map"/>
-        <param name="map" value="$(find aruco_pose)/map/map.txt"/>
-        <param name="snap_orientation" value="local_origin"/>
-    </node>
+    <node pkg="nodelet" type="nodelet" name="aruco_vpe" args="load clever/aruco_vpe nodelet_manager" clear_params="true">
+        <param name="aruco_orientation" value="local_origin"/>
+        <!--<param name="aruco_orientation" value="local_origin_upside_down"/>-->
 
-    <!-- vpe publisher from aruco markers -->
-    <node name="vpe_publisher" pkg="clever" type="vpe_publisher" output="screen" if="$(arg aruco_vpe)">
-        <!-- <remap from="~markers" to="aruco_detect/markers"/> -->
-        <!-- <remap from="~pose_pub" to="mavros/mocap/pose"/> --> <!-- publish MOCAP -->
-        <remap from="~pose_cov" to="aruco_map/pose"/>
-        <remap from="~pose_pub" to="mavros/vision_pose/pose"/> <!-- publish VPE -->
-        <param name="frame_id" value="aruco_map"/>
-        <param name="child_frame_id" value="fcu"/>
-        <param name="publish_zero" value="true"/>
+        <param name="use_mocap" value="true"/>
     </node>
 </launch>

clever/launch/clever.launch

@@ -22,7 +22,7 @@
     <!-- web video server -->
     <node name="web_video_server" pkg="web_video_server" type="web_video_server" if="$(arg web_video_server)" required="false" respawn="true" respawn_delay="5"/>
 
-    <!-- aruco -->
+    <!-- aruco vpe -->
     <include file="$(find clever)/launch/aruco.launch" if="$(arg aruco)"/>
 
     <!-- optical flow -->
@@ -33,7 +33,7 @@
 
     <!-- main nodelet manager -->
     <node pkg="nodelet" type="nodelet" name="nodelet_manager" args="manager" output="screen" clear_params="true">
-        <param name="num_worker_threads" value="4"/>
+        <param name="num_worker_threads" value="2"/>
     </node>
 
     <node pkg="tf2_ros" type="static_transform_publisher" name="local_origin_upside_down_frame" args="0 0 0 3.1415926 3.1415926 0 local_origin local_origin_upside_down"/>

clever/launch/main_camera.launch

@@ -2,13 +2,18 @@
     <!-- Camera position and orientation are represented by fcu -> main_camera_optical transform -->
     <!-- static_transform_publisher arguments: x y z yaw pitch roll frame_id child_frame_id -->
 
-    <!-- clever 2 -->
-    <!--<node pkg="tf2_ros" type="static_transform_publisher" name="main_camera_frame" args="0 0 -0.07 -1.5707963 0 3.1415926 fcu main_camera_optical"/>-->
+    <!-- article about camera setup: https://clever.copterexpress.com/camera_frame.html -->
 
-    <!-- clever 3 -->
-    <node pkg="tf2_ros" type="static_transform_publisher" name="main_camera_frame" args="0.05 0 -0.07 1.5707963 0 3.1415926 fcu main_camera_optical"/>
+    <!-- camera is oriented downward, camera cable goes backward [option 1] -->
+    <node pkg="tf2_ros" type="static_transform_publisher" name="main_camera_frame" args="0.05 0 -0.07 -1.5707963 0 3.1415926 fcu main_camera_optical"/>
 
-    <!-- clever 3, upwards -->
+    <!-- camera is oriented downward, camera cable goes forward  [option 2] -->
+    <!--<node pkg="tf2_ros" type="static_transform_publisher" name="main_camera_frame" args="0.05 0 -0.07 1.5707963 0 3.1415926 fcu main_camera_optical"/>-->
+
+    <!-- camera is oriented upward, camera cable goes backward   [option 3] -->
+    <!--<node pkg="tf2_ros" type="static_transform_publisher" name="main_camera_frame" args="0.05 0 0.07 1.5707963 0 0 fcu main_camera_optical"/>-->
+
+    <!-- camera is oriented upward, camera cable goes forward    [option 4] -->
     <!--<node pkg="tf2_ros" type="static_transform_publisher" name="main_camera_frame" args="0.05 0 0.07 -1.5707963 0 0 fcu main_camera_optical"/>-->
 
     <!-- camera node -->

clever/src/selfcheck.py

@@ -9,7 +9,7 @@ from std_srvs.srv import Trigger
 from sensor_msgs.msg import Image, CameraInfo, NavSatFix, Imu, Range
 from mavros_msgs.msg import State, OpticalFlowRad
 from geometry_msgs.msg import PoseStamped, TwistStamped
-from aruco_pose.msg import MarkerArray
+import tf.transformations as t
 
 
 # TODO: roscore is running
@@ -56,9 +56,9 @@ def check_fcu():
     try:
         state = rospy.wait_for_message('mavros/state', State, timeout=3)
         if not state.connected:
-            failure('No connection to the FCU (check wiring)')
+            failure('no connection to the FCU (check wiring)')
     except rospy.ROSException:
-        failure('No MAVROS state (check wiring)')
+        failure('no MAVROS state (check wiring)')
 
 
 @check('Camera')
@@ -66,37 +66,58 @@ def check_camera(name):
     try:
         img = rospy.wait_for_message(name + '/image_raw', Image, timeout=1)
     except rospy.ROSException:
-        failure('%s: No images (is the camera connected properly?)', name)
+        failure('%s: no images (is the camera connected properly?)', name)
         return
     try:
         info = rospy.wait_for_message(name + '/camera_info', CameraInfo, timeout=1)
     except rospy.ROSException:
-        failure('%s: No calibration info', name)
+        failure('%s: no calibration info', name)
         return
 
     if img.width != info.width:
-        failure('%s: Calibration width doesn\'t match image width (%d != %d)', name, info.width, img.width)
+        failure('%s: calibration width doesn\'t match image width (%d != %d)', name, info.width, img.width)
     if img.height != info.height:
-        failure('%s: Calibration height doesn\'t match image height (%d != %d))', name, info.height, img.height)
+        failure('%s: calibration height doesn\'t match image height (%d != %d))', name, info.height, img.height)
 
 
 @check('Aruco detector')
 def check_aruco():
     try:
-        rospy.wait_for_message('aruco_detect/markers', MarkerArray, timeout=0.5)
+        rospy.wait_for_message('aruco_pose/debug', Image, timeout=1)
     except rospy.ROSException:
-        failure('No aruco markers detection')
+        failure('no aruco_pose/debug messages')
 
 
-@check('Visual position estimate')
+@check('Vision position estimate')
 def check_vpe():
     try:
-        rospy.wait_for_message('mavros/vision_pose/pose', PoseStamped, timeout=1)
+        vis = rospy.wait_for_message('mavros/vision_pose/pose', PoseStamped, timeout=1)
     except rospy.ROSException:
         try:
-            rospy.wait_for_message('mavros/mocap/pose', PoseStamped, timeout=1)
+            vis = rospy.wait_for_message('mavros/mocap/pose', PoseStamped, timeout=1)
         except rospy.ROSException:
-            failure('No VPE or MoCap messages')
+            failure('no VPE or MoCap messages')
+            return
+
+    # check vision pose and estimated pose inconsistency
+    try:
+        pose = rospy.wait_for_message('mavros/local_position/pose', PoseStamped, timeout=1)
+    except:
+        return
+    horiz = math.hypot(vis.pose.position.x - pose.pose.position.x, vis.pose.position.y - pose.pose.position.y)
+    if horiz > 0.5:
+        failure('horizontal position inconsistency: %.2f m', horiz)
+    vert = vis.pose.position.z - pose.pose.position.z
+    if abs(vert) > 0.5:
+        failure('vertical position inconsistency: %.2f m', vert)
+    op = pose.pose.orientation
+    ov = vis.pose.orientation
+    yawp, _, _ = t.euler_from_quaternion((op.x, op.y, op.z, op.w), axes='rzyx')
+    yawv, _, _ = t.euler_from_quaternion((ov.x, ov.y, ov.z, ov.w), axes='rzyx')
+    yawdiff = yawp - yawv
+    yawdiff = math.degrees((yawdiff + 180) % 360 - 180)
+    if abs(yawdiff) > 8:
+        failure('yaw inconsistency: %.2f deg', yawdiff)
 
 
 @check('Simple offboard node')
@@ -106,7 +127,7 @@ def check_simpleoffboard():
         rospy.wait_for_service('get_telemetry', timeout=3)
         rospy.wait_for_service('land', timeout=3)
     except rospy.ROSException:
-        failure('No simple_offboard services')
+        failure('no simple_offboard services')
 
 
 @check('IMU')
@@ -114,15 +135,25 @@ def check_imu():
     try:
         rospy.wait_for_message('mavros/imu/data', Imu, timeout=1)
     except rospy.ROSException:
-        failure('No IMU data (check flight controller calibration)')
+        failure('no IMU data (check flight controller calibration)')
 
 
 @check('Local position')
 def check_local_position():
     try:
-        rospy.wait_for_message('mavros/local_position/pose', PoseStamped, timeout=1)
+        pose = rospy.wait_for_message('mavros/local_position/pose', PoseStamped, timeout=1)
+        o = pose.pose.orientation
+        _, pitch, roll = t.euler_from_quaternion((o.x, o.y, o.z, o.w), axes='rzyx')
+        MAX_ANGLE = math.radians(2)
+        if abs(pitch) > MAX_ANGLE:
+            failure('pitch is %.2f deg; place copter horizontally or redo level horizon calib',
+                    math.degrees(pitch))
+        if abs(roll) > MAX_ANGLE:
+            failure('roll is %.2f deg; place copter horizontally or redo level horizon calib',
+                    math.degrees(roll))
+
     except rospy.ROSException:
-        failure('No local position')
+        failure('no local position')
 
 
 @check('Velocity estimation')
@@ -132,23 +163,23 @@ def check_velocity():
         horiz = math.hypot(velocity.twist.linear.x, velocity.twist.linear.y)
         vert = velocity.twist.linear.z
         if abs(horiz) > 0.1:
-            failure('Horizontal velocity estimation is %.2f m/s; is copter staying still?' % horiz)
+            failure('horizontal velocity estimation is %.2f m/s; is copter staying still?' % horiz)
         if abs(vert) > 0.1:
-            failure('Vertical velocity estimation is %.2f m/s; is copter staying still?' % vert)
+            failure('vertical velocity estimation is %.2f m/s; is copter staying still?' % vert)
 
         angular = velocity.twist.angular
         ANGULAR_VELOCITY_LIMIT = 0.01
         if abs(angular.x) > ANGULAR_VELOCITY_LIMIT:
-            failure('Pitch rate estimation is %.2f rad/s (%.2f deg/s); is copter staying still?',
+            failure('pitch rate estimation is %.2f rad/s (%.2f deg/s); is copter staying still?',
                     angular.x, math.degrees(angular.x))
         if abs(angular.y) > ANGULAR_VELOCITY_LIMIT:
-            failure('Pitch rate estimation is %.2f rad/s (%.2f deg/s); is copter staying still?',
+            failure('pitch rate estimation is %.2f rad/s (%.2f deg/s); is copter staying still?',
                     angular.y, math.degrees(angular.y))
         if abs(angular.z) > ANGULAR_VELOCITY_LIMIT:
-            failure('Pitch rate estimation is %.2f rad/s (%.2f deg/s); is copter staying still?',
+            failure('pitch rate estimation is %.2f rad/s (%.2f deg/s); is copter staying still?',
                     angular.z, math.degrees(angular.z))
     except rospy.ROSException:
-        failure('No velocity estimation')
+        failure('no velocity estimation')
 
 
 @check('Global position (GPS)')
@@ -156,7 +187,7 @@ def check_global_position():
     try:
         rospy.wait_for_message('mavros/global_position/global', NavSatFix, timeout=1)
     except rospy.ROSException:
-        failure('No global position')
+        failure('no global position')
 
 
 @check('Optical flow')
@@ -165,7 +196,7 @@ def check_optical_flow():
     try:
         rospy.wait_for_message('mavros/px4flow/raw/send', OpticalFlowRad, timeout=0.5)
     except rospy.ROSException:
-        failure('No optical flow data (from Raspberry)')
+        failure('no optical flow data (from Raspberry)')
 
 
 @check('Rangefinder')
@@ -174,11 +205,11 @@ def check_rangefinder():
     try:
         rospy.wait_for_message('mavros/distance_sensor/rangefinder_3_sub', Range, timeout=0.5)
     except rospy.ROSException:
-        failure('No randefinder data from Raspberry')
+        failure('no randefinder data from Raspberry')
     try:
         rospy.wait_for_message('mavros/distance_sensor/rangefinder_0', Range, timeout=0.5)
     except rospy.ROSException:
-        failure('No rangefinder data from PX4')
+        failure('no rangefinder data from PX4')
 
 
 @check('Boot duration')
@@ -206,7 +237,7 @@ def check_cpu_usage():
         pid, cpu, cmd = process.split('\t')
 
         if cmd.strip() not in WHITELIST and float(cpu) > 30:
-            failure('High CPU usage (%s%%) detected: %s (PID %s)',
+            failure('high CPU usage (%s%%) detected: %s (PID %s)',
                     cpu.strip(), cmd.strip(), pid.strip())
 
 

clever/src/utils.h

@@ -1,36 +0,0 @@
-#pragma once
-// TODO: merge with util.h
-
-#include <string>
-#include <ros/ros.h>
-#include <geometry_msgs/Vector3.h>
-#include <geometry_msgs/Point.h>
-
-// Read required param or shutdown the node
-template<typename T>
-static void param(ros::NodeHandle nh, const std::string& param_name, T& param_val)
-{
-	if (!nh.getParam(param_name, param_val)) {
-		ROS_FATAL("Required param %s is not defined", param_name);
-		ros::shutdown();
-	}
-}
-
-static inline double hypot(double x, double y, double z)
-{
-	return std::sqrt(x*x + y*y + z*z);
-}
-
-static inline void vectorToPoint(const geometry_msgs::Vector3& vector, geometry_msgs::Point& point)
-{
-	point.x = vector.x;
-	point.y = vector.y;
-	point.z = vector.z;
-}
-
-static inline void pointToVector(const geometry_msgs::Point& point, geometry_msgs::Vector3& vector)
-{
-	vector.x = point.x;
-	vector.y = point.y;
-	vector.z = point.z;
-}

clever/src/vpe_publisher.cpp

@@ -1,99 +0,0 @@
-/*
- * Universal VPE publisher node
- * Copyright (C) 2018 Copter Express Technologies
- *
- * Author: Oleg Kalachev <okalachev@gmail.com>
- *
- * Distributed under MIT License (available at https://opensource.org/licenses/MIT).
- * The above copyright notice and this permission notice shall be included in all
- * copies or substantial portions of the Software.
- */
-
-#include <string>
-#include <ros/ros.h>
-#include <tf2_ros/buffer.h>
-#include <tf2_ros/transform_listener.h>
-#include <tf2_ros/transform_broadcaster.h>
-#include <geometry_msgs/TransformStamped.h>
-#include <geometry_msgs/PoseStamped.h>
-#include <geometry_msgs/PoseWithCovarianceStamped.h>
-#include <aruco_pose/MarkerArray.h>
-
-#include "utils.h"
-
-using std::string;
-
-static string frame_id, child_frame_id;
-static tf2_ros::Buffer tf_buffer;
-static ros::Publisher vpe_pub;
-static ros::Subscriber local_position_sub;
-static ros::Timer zero_timer;
-static geometry_msgs::PoseStamped vpe, pose;
-static ros::Time local_pose_stamp(0);
-static ros::Duration publish_zero_timout;
-tf2_ros::TransformBroadcaster br;
-
-void publishZero(const ros::TimerEvent&)
-{
-	if ((ros::Time::now() - pose.header.stamp < publish_zero_timout) ||
-	    (ros::Time::now() - vpe.header.stamp < publish_zero_timout))
-		return;
-
-	ROS_INFO_THROTTLE(10, "vpe_publisher: publish zero");
-	vpe.header.stamp = ros::Time::now();
-	vpe.pose.orientation.w = 1;
-	vpe_pub.publish(vpe);
-}
-
-void localPositionCallback(const geometry_msgs::PoseStamped& msg)
-{
-	pose = msg;
-}
-
-template <typename T>
-void callback(const T& msg)
-{
-	try {
-		auto transform = tf_buffer.lookupTransform(frame_id, child_frame_id,
-		                                            msg->header.stamp, ros::Duration(0.02));
-		                                            vpe.header.stamp = msg->header.stamp;
-		vpe.pose.position.x = transform.transform.translation.x;
-		vpe.pose.position.y = transform.transform.translation.y;
-		vpe.pose.position.z = transform.transform.translation.z;
-		vpe.pose.orientation = transform.transform.rotation;
-		vpe.header.stamp = msg->header.stamp;
-		vpe_pub.publish(vpe);
-	} catch (const tf2::TransformException& e) {
-		ROS_WARN_THROTTLE(10, "vpe_publisher: %s", e.what());
-	}
-}
-
-int main(int argc, char **argv) {
-	ros::init(argc, argv, "vpe_publisher");
-	ros::NodeHandle nh, nh_priv("~");
-
-	tf2_ros::TransformListener tf_listener(tf_buffer);
-
-	param(nh_priv, "frame_id", frame_id);
-	param(nh_priv, "child_frame_id", child_frame_id);
-	nh_priv.param<std::string>("mavros/local_position/frame_id", vpe.header.frame_id, "map");
-
-	auto pose_sub = nh_priv.subscribe<geometry_msgs::PoseStamped>("pose", 1, &callback);
-	auto pose_cov_sub = nh_priv.subscribe<geometry_msgs::PoseWithCovarianceStamped>("pose_cov", 1, &callback);
-	auto markers_sub = nh_priv.subscribe<aruco_pose::MarkerArray>("markers", 1, &callback);
-	//auto pose_cov_sub = nh_priv.subscribe<geometry_msgs::PoseWithCovarianceStamped>("pose_cov", 1, &callback);
-
-	vpe_pub = nh_priv.advertise<geometry_msgs::PoseStamped>("pose_pub", 1);
-	//vpe_cov_pub = nh_priv_.advertise<geometry_msgs::PoseStamped>("pose_cov_pub", 1);
-
-	if (nh_priv.param("publish_zero", false)) {
-		// publish zero to initialize the local position
-		vpe.header.stamp = ros::Time(0);
-		zero_timer = nh.createTimer(ros::Duration(0.1), &publishZero);
-		publish_zero_timout = ros::Duration(nh_priv.param("publish_zero_timout", 5.0));
-		local_position_sub = nh.subscribe("mavros/local_position/pose", 1, &localPositionCallback);
-	}
-
-	ROS_INFO("vpe_publisher: ready");
-	ros::spin();
-}

docs/LANGS.md

@@ -0,0 +1,4 @@
+# Languages
+
+* [Русский](ru/)
+* [English](en/)

docs/SUMMARY.md

@@ -1,54 +0,0 @@
-# Summary
-
-* [Введение](README.md)
-* [Глоссарий](glossary.md)
-* [Сборка Клевер 2](assemble.md)
-* [Сборка Клевер 3](assemble_clever3_4in1.md)
-* [Первоначальная настройка](setup.md)
-* [Полетные режимы](modes.md)
-* [Raspberry Pi](raspberry.md)
-* [Образ операционной системы на RPi](microsd_images.md)
-* [Подключение Raspberry Pi к Pixhawk](connection.md)
-* [Подключение по Wi-Fi](wifi.md)
-* [Работа с QGroundControl через Wi-Fi](gcs_bridge.md)
-* [Прошивка Pixhawk/Pixracer](firmware.md)
-* [Параметры PX4](px4_parameters.md)
-* [Пилотирование со смартфона](rc.md)
-* [SSH-доступ](ssh.md)
-* [Устройство UART](uart.md)
-* [Неисправности радиоаппаратуры](radioerrors.md)
-* [Безопасность](safety.md)
-* [Техника безопасности по пайке](tb.md)
-* [Просмотр видеострима с камер](web_video_server.md)
-* [Работа с ROS](ros.md)
-* [MAVROS](mavros.md)
-* [Автономный полет в OFFBOARD](simple_offboard.md)
-* [Примеры программ](snippets.md)
-* [Навигация по ArUco-маркерам](aruco.md)
-* [Взаимодействие с Arduino](arduino.md)
-* [Системы координат](frames.md)
-* [Работа с камерой \(компьютерное зрение\)](camera.md)
-* [Ориентация камеры](camera_frame.md)
-* [Визуализация с помощью rviz](rviz.md)
-* [Работа с SITL](sitl.md)
-* [Подключение GPS](gps.md)
-* [Автозапуск ПО](autolaunch.md)
-* [Использование 3G-модема](3g.md)
-* [Устройство сети RPi](network.md)
-* [Работа с логами PX4](flight_logs.md)
-* [Протокол MAVLink](mavlink.md)
-* [Типы силовых разъемов](connectortypes.md)
-* [Ошибки радиоаппаратуры](radioerrors1.md)
-* [Настройка PID](calibratePID.md)
-* Учебник
-  * [Теория и видеоуроки](lessons.md)
-  * [Учебно-методическое пособие](metod.md)
-  * [Контрольные и проверочные материалы](tests.md)
-* [Другое](drugoe.md)
-  * [CopterHack-2017](copterhack2017.md)
-  * [Прошивка ESC контроллеров с помощью Arduino](esc_firmware.md)
-  * [Работа со светодиодной лентой](leds.md)
-  * [Проекты на базе коптера "Клевер"](projects.md)
-  * [Тестовое описание Клевера по шаблону robots.ros.org/gapter/](testovoe-opisanie-klevera-po-shablonu-robotsrosorggapter.md)
-* [Полезные ссылки](links.md)
-

docs/assemble_clever3_4in1.md

@@ -1,154 +0,0 @@
-# Инструкция по сборке конструктора Клевер 3
-
-В данной инструкции рассматривается сборка комплекта COEX Clever 3 с платой регуляторов 4в1
-
-![Clever](assets/clever3_main.jpg)
-
-## Состав конструктора
-
-TODO
-
-## Дополнительное оборудование
-
-![Дополнительное оборудование](assets/additonal_eqipment.jpg)
-
-## Условное обозначение
-
-![Условные обозначения](assets/conditional_refer.jpg)
-
-## Техника безопасности при пайке
-
-Перед использованием паяльного оборудования обязательно ознакомьтесь с техникой безопасности
-[Техника безопасности при пайке](tb.md)
-
-## Установка моторов
-
-1. Распаковать моторы
-2. Закрепить мотор на луче шестигранными винтами М3х6 (самые короткие винты в комплекте с моторами). *Шестигранный ключ в комплекте.
-3. Вставить гайки М3 (4 шт) в пластиковый держатель.
-    > Для удобства можно использовать длинный винт, либо плоскогубцы
-4. Закрепить луч, нижнюю защиту луча и держатель винтами М3х12, используя крестовую отвертку.
-5. Скрепить хомутом луч и нижнюю защиту луча.
-    > Хвост от хомута (стяжки) отрезать ножницами ![Подготовка моторов](assets/cl3_prepareMotors.JPG)
-
-## Монтаж каркасных элементов
-
-1. Установить пластиковые гайки М3 (4 шт) для крепления PDB на раму винтами М3х8
-2. Установить стойки 6 мм (4 шт) для крепления Raspberry Pi на раму винтами М3х8
-3. Установить на раму собранную конструкцию, соблюдая схему, винтами М3х16
-4. Установить каркас для светодиодной ленты, используя прорези в держателях для ножек
-
-![Монтаж стоек на раму](assets/cl3_mountElements.JPG)
-
-## Монтаж преобразователя напряжения BEC (Припаять и проверить)
-
-1. Распаковать плату питания и установить шлейф питания
-2. Включить мультиметр в режим измерения постоянного напряжения (диапазон 20В или 200В)
-3. Проверяем работоспособность платы питания, подключив АКБ
-   * a. Выходное напряжение на разъеме XT30 должно равняться напряжению на АКБ (от 10В до 12.6В)
-   * b. Выходное напряжение на шлейфе питания должно быть в пределах 4.9В до 5.3В
-       > Измеряем между черным и красным проводами
-4. Распаковываем преобразователь напряжения и снимаем прозрачную изоляцию
-5. Припаиваем два дополнительных провода на BEC
-   * a. Берем из набора 3 провода папа-мама (красный, черный и любого цвета)
-   * b. Красный и черный [залуживаем](zap.md) с обеих сторон, используя пинцет. На синем проводе залуживаем со стороны коннектора ПАПА.
-     Залудить - это:
-     * Нанести флюс на оголенную часть провода.
-     * Покрыть припоем.
-   * c. Припаиваем красный и черный провода к BEC: ЧЕРНЫЙ -> OUT-, КРАСНЫЙ -> OUT+
-6. Проверяем работу BEC
-   * a. Припаиваем BEC на плату питания. ЧЕРНЫЙ -> -, КРАСНЫЙ -> +
-   * b. Подключаем АКБ и проверяем напряжение на припаянных проводах к BEC (из пункта 5) 5В - все правильно! больше 10В - отключите питание и переставьте желтую перемычку на другой пинцет, 0В - плохо спаяли
-   * с. Если BEC выдает 5В, то изолируем паячное соединение черной термоусадкой.
-7. Монтаж светодиодной ленты. Припаять провода от BEC (из пункта 5) к светодиодной ленте
-   * a. Удалить силиконовый слой на ленте (надрезать ножом и оторвать)
-   * b. [Залудить](zap.md) контакты светодиодной ленты
-   * c.
-     * Красный -> +5V
-     * Черный ->  GND
-     * Синий -> Din
-
-![Монтаж преобразователя напряжения BEC ](assets/cl3_mountBEC.JPG)
-
-## Монтаж платы регуляторов 4в1 и платы питания PDB
-
-1. Установить плату регуляторов 4в1 как показано на картинке. Соединить фазные провода моторов с проводами регуляторов.
-2. Закрепить плату регуляторов стойками 6 мм (4 шт.). На стойки накрутить пластиковые гайки М3 (4 шт.).
-3. Установить плату распределения питания PDB как показано на картинке (разъем XT60 направлен к хвосту коптера).
-4. Соединить разъемы питания платы питания и платы регуляторов XT30.
-
-![Монтаж платы питания](assets/cl3_mountESC.JPG)
-
-## Сопряжение приемника и пульта
-
-1. Подключаем провод 5В от BEC в разъем приемника. Устанавливаем BIND разъем в крайний правый порт B/VCC
-2. Подключаем АКБ. Индикатор на приемники должен быстро мигать (режим сброса).
-3. Зажимаем и удерживаем кнопку BIND на пульте и включаем пульт. На пульте отображается процесс сопряжения RXBinding.
-4. После установки сопряжения (появление доп строк на дисплее пульта), убираем BIND разъем из приемника. Отключаем АКБ.
-
-![Сопряжение приемника и пульта](assets/cl3_bindFlysky.JPG)
-
-> Если пульт не включается или заблокирован, то смотри здесь
-[Неисправности пульта](radioerrors1.md)
-
-## Проверка направления вращения моторов
-
-1. Включить пульт. Убедиться, что ppm в меню RX Setup отключен ([раздел "Нет связи с полетным контроллером"](radioerrors1.md) в пункте 3 выберите “RX setup” > “PPM OUTPUT” > “Off”. Сохраните изменения (удерживаем нажатой кнопку “CANCEL”)
-2. Подключите оранжевый провод S1 от платы регуляторов в CH3 на приемнике. Подключить внешнее питание.
-3. Подать левым стиком газ (throttle) на 10%.
-4. Проверить направления вращения мотора по схеме.Повторить для каждого мотора. Таким образом, будет понятно каким именно мотором мы управляем
-5. Если необходимо изменить направление вращения, то меняем любые два фазных провода мотора (нужно переподключить).
-
-![Проверка направления вращения моторов](assets/cl3_testMotorsFlysky.JPG)
-
-## Монтаж и подключение полетного контроллера PixRacer
-
-1. Установить Полетный контроллер PixRacer на двухстороний скотч 3М (2-3 слоя). *также полетный контроллер можно извлечь из корпуса и жестко установить на стойки М3х6
-
-2. Установить стойки 40 мм, используя винты М3х8. Подключить разъем POWER
-
-3. Подключить регуляторы, как на картинке. Подробно [про подключение регуляторов 4в1](cl3_connectESC4in1.md)
-4. Подключить шлейф радиоприемника в разъем RCIN в PixRacer ![Монтаж полетного контроллера](assets/cl3_mountPixracer.JPG)
-
-## Монтаж Raspberry Pi
-
-1. Перевернуть коптер. Установить Raspberry на стойки, используя монтажные отверстия Raspberry. USB-разъемы направлены к хвостовой части коптера
-2. Установка шлейфа для камеры
-    * a. поднять защелку
-    * b. подключить шлейф
-    * c. закрыть защелку
-3. Подключение питания Raspberry
-    * 5В -> pin 04 (DC power 5v)
-    * GND -> pin 06 (Ground)
-    * Подключение светодиодной ленты pin 40 (GPIO21)
-4. Сборка маунта для камеры Raspberry Pi. Используйте винт М3х16 и гайку М3. ![Монтаж Raspberry Pi Model B](assets/cl3_mountRaspberryPi.JPG)
-
-## Монтаж Arduino и радиоприемника FlySky
-
-1. Произведите монтаж пинов микроконтроллера Arduino Nano, используя пайку
-2. Установить миконтроллер в специальной маунт и прикрепите к нижней деке, используя винты М3х16 (4 шт.)
-3. Используя 2хсторонний скотч прикрепите приемник, как показано на рисунке
-4. Подключите шлейф радиоприемника от PixRacer как на рисунке:
-    * белый -> PPM
-    * красный -> 5V
-    * черный -> GND
-    * оранжевый, зеленый -> сейчас не используются. Устанавливаются в неиспользуемые пины радиоприемника. ![Монтаж Arduino nano и радиоприемника Flysky i6](assets/cl3_mountArduinoandFlysky.JPG)
-
-## Монтаж камеры Raspberry Pi
-
-1. Установить маунт для камеры Raspberry Pi в сборе на нижнюю деку винтами М3х12 (2 шт.)
-2. Подключить шлейф к камере Raspberry Pi
-3. Установить камеру в маунт, закрепить саморезами М2
-4. Закрепить Raspberry стойками 30 мм (4 шт.). Установить нижнюю деку в сборе на стойки винтами М3х8 (4шт.).
-5. Установить ножки в маунты (4 шт.) ![Монтаж камеры RPi](assets/cl3_mountRpiCamera.JPG).
-
-## Монтаж остальных конструктивных элементов
-
-1. Установка нижней защиты, используя винты М3х12 (8 шт.) и стойки 30 мм (8 шт.)
-2. Установка верхней защиты, используя винты М3х12 (8 шт.)
-3. Установка ремешка верхнюю деку для фиксации АКБ. Закрепить верхнюю деку винтами М3х8 (4 шт.). ![Монтаж остальных конструктивных элементов](assets/cl3_mountOtherElements.JPG)
-
-## Монтаж USB соединителей
-
-1. Соедините PixRacer и Raspberry Pi, используя micro USB - USB кабель
-2. Соедините Arduino и Raspberry Pi, используя micro USB - USB кабель. ![Монтаж USB соединителей](assets/cl3_mountUSBconnectors.JPG)

docs/bundle.md

@@ -1,31 +0,0 @@
-Пакет программ на Raspberry Pi
-===
-
-Пакет программ clever_bundle, устанавливающийся на Raspberry Pi, позволяет:
-
-* [Настраивать и управлять коптером используя QGroundControl с соединением по Wi-Fi](gcs_bridge.md)
-* [Использовать веб-пульт управления квадрокоптером](web_rc.md)
-* [Получать доступ к Raspberry Pi при помощи SSH](ssh.md)
-* Анализировать полеты квадрокоптера с помощью RViz и RosBag
-* [Работать с камерой для CV](camera.md)
-* Работать с камерой для FPV
-* [Управлять полетом коптера программно, используя модуль offboard](offboard.md)
-* [Осуществлять навигацию в поле ArUco-маркеров](aruco.md)
-* [Использовать внешний 3G-модем для осуществление связи коптера с Интернетом](3g.md)
-* Разрабатывать произвольные модули и системы
-
-Установка clever_bundle
----
-
-Для установка пакета clever_bundle необходимо скачать последнюю версию образа SD-карты и загрузить его на флешку, например, используя программу [Etcher](https://etcher.io).
-
-Информация
----
-
-Образ SD-карты включает в себя:
-
-* ОС [Raspbian Jessie](https://www.raspberrypi.org/downloads/raspbian/)
-* Фреймворк [ROS](ros.md)
-* Пакет [MAVROS](mavros.md) для связи с Pixhawk по [MAVLink](mavlink.md)
-* Дополнительные пакеты ROS: web_video_server, usb_cam, rosbridge_suite и другие
-* Пакет программ clever_bundle

docs/drugoe.md

@@ -1,2 +0,0 @@
-
-

docs/en/README.md

@@ -0,0 +1,3 @@
+# CLEVER
+
+This is CLEVER drone kit documentation in English.

docs/en/SUMMARY.md

@@ -0,0 +1,3 @@
+# Summary
+
+* [UART settings](uart.md)

docs/en/book.json

@@ -0,0 +1,4 @@
+{
+    "language": "en",
+    "root": "."
+}

docs/en/uart.md

@@ -0,0 +1,86 @@
+# UART interface
+
+UART is an asynchronous serial interface for data transfer that is used in many devices. For example, GPS antennas, Wi-Fi routers, or Pixhawk.
+
+The interface usually contains two lines: TX for data transmission, and RX for data reception. It usually uses the 5-volt logics.
+
+To connect two devices, you have to feed the TX line of the first device to the RX line of the other one. A similar manipulation is required on the other end for ensuring two-way data transmission.
+
+It is also necessary to synchronize the voltages – connect the ground on two devices.
+
+Read more about the interface and the Protocol in [this article](https://habr.com/post/109395/).
+
+## Linux TTY
+
+In Linux, there is the concept of Posix Terminal Interface (read more [here](https://ru.wikipedia.org/wiki/TTY-абстракция)). It is an abstraction over the serial or virtual interface that allows several agents to work with the device simultaneously.
+
+An example of such abstraction in Raspbian may be `/dev/tty1` – the device for text output to the screen connected via HDMI.
+
+## UART on Raspberry Pi 3
+
+Raspberry Pi 3 has two hardware UART interfaces:
+
+1. Mini UART (`/dev/ttyAMA0`) – uses the timing of the RPi graphics core, and therefore limits its frequency.
+2. PL011 (`/dev/ttyS0`) – the full-fledged UART interface on a separate chip of the microcontroller.
+
+Read more about UART on Raspberry Pi in the [official article](https://www.raspberrypi.org/documentation/configuration/uart.md).
+
+Using microcontroller valves, these interfaces may be switched between two physical outputs:
+
+1. UART connector on GPIO;
+2. RPi Bluetooth module.
+
+By default, Raspberry Pi 3 PL011 is connected to the Bluetooth module. And Mini UART is disabled with the value of directive `enable_uart`, which is `0` by default.
+
+> **Note** One should understand that directive `enable_uart` changes its default value, depending on which UART is connected to the RPi Bluetooth module with directive `dtoverlay=pi3-miniuart-bt`.
+
+For the sake of convenience of working with these outputs, aliases exist in Raspbian:
+
+* `/dev/serial0` – always points to the TTY device that is connected to the GPIO ports.
+* `/dev/serial1` – always points to the TTY device that is connected to the Bluetooh module.
+
+### Configuration of UART on Raspberry Pi 3
+
+To configure UART, there are directives located in `/boot/config.txt`.
+
+To enable the UART interface on GPIO:
+
+```txt
+enable_uart=1
+```
+
+To disconnect the UART interface from the Bluetooth module:
+
+```txt
+dtoverlay=pi3-disable-bt
+```
+
+To connect Mini UART to the Bluetooth module:
+
+```txt
+dtoverlay=pi3-miniuart-bt
+```
+
+If the Bluetooth module is disabled, one should disable the hciuart service:
+
+```bash
+sudo systemctl disable hciuart.service
+```
+
+## Default image configuration
+
+In image CLEVER, we initially disabled Mini UART and the Bluetooth module.
+
+Bugs
+----
+
+If you use the Mini UART connection to Bluetooth, `hciuart` crashes with the following error:
+
+![hciuart error](../assets/hciuart_error.jpg)
+
+In case of Bluetooth disconnection
+
+```txt
+/dev/serial0 -\> ttyAMA0
+/dev/serial1 -\> ttyS0
+```

docs/etcher.md

@@ -1,4 +0,0 @@
-Использование программы Etcher
-===
-
-TODO

docs/flip.md

@@ -1,2 +0,0 @@
-# Автоматический флип
-

docs/links.md

@@ -1,2 +0,0 @@
-Полезные ссылки
-===

docs/offboard.md

@@ -1,12 +0,0 @@
-Управление коптером в режиме OFFBOARD
-===
-
-Координатные системы (фреймы)
----
-
-TODO
-
-* `local_origin` — координаты точки инициализации полетного контроллера
-* `fcu` (flight control unit) — координаты квадрокоптера с учетом наклонов
-* `fcu_horiz` — координаты квадрокоптера без учета наклонов по тангажу и рысканью
-* `marker_map` — координаты поля ArUco-маркеров

docs/powerConnection.md

@@ -1,23 +0,0 @@
-Силовые и управляющие цепи
-===============================
-
-Силовые цепи
---------------------
-Из вырезки по ГОСТ 18311-80
-
-"Электрическая цепь, содержащая элементы, функциональное назначение которых состоит в производстве или передаче основной части электрической энергии,
-ее распределении, преобразовании в другой вид энергии или в электрическую энергию с другими значениями параметров."
-
-Говоря простым языком, силовые цепи - это электрические цепи, которые предназначены для передачи большого количества энергии (тока), для обеспечения работоспособности всех систем.
-В нашем случае в качестве силовой цепи будут выступать провода, соединяющие аккумулятор с платой распределения питания, а также красные и черные провода идущие на регуляторы оборотов. 
-
-![Схема силовой цепи](assets/powerConnect.jpg)
-
-Вся энергия аккумулятора (АКБ) будет распределяться между регуляторами оборотов моторов. Чтобы моторы смогли поднять в воздух коптер, им необходимо много энергии. В качестве энергии выступает ток, который приходит с АКБ. Т.к. энергии нужно много, значит по проводам будет идти большой ток. Чтобы провода смогли выдержать такую нагрузку, необходимы  провода большего диаметра. Для наших задач подойдут провода 18AWG, 16AWG, 14AWG.
-
-
-
-
-Управляющие цепи
---------------------
-

docs/radioerrors1.md

@@ -1,47 +0,0 @@
-Мануал по возможным неисправностям радиоаппаратуры
-==========================================
-
-Пульт  заблокирован
---------------
-Если пульт заблокирован, то на ЖК Экране будет отображено предупреждение:
-"Warning. Place all switches in their up position and lower the throttle".
-
-Для разблокировки пульта необходимо привести все стики и переключатели в исходное положение, а именно:
-
-1. Левый стик (1) в центральной нижней позиции. 
-2. Переключатели A, B, C, D (2) в положение “От Себя”.
-3. Правый стик (3)  в центре.
-
-![Заблокированный пульт](assets/lockradio.jpg)
-
-
-Нет связи с приемником
---------------
-Для проверки соединения пульта с приемником, включаем пульт и смотрим индикацию на ЖК Экране:
-
-1. Соединение с приемником отсутствует
-
-![Нет соединения с приемником](assets/connectionLost.jpg)
-
-2. Соединение с приемником установлено
-
-![Есть соединения с приемником](assets/connectionOK.jpg)
-
-Если соединение отсутствует, то
-1. Проверьте, что приемник включен (моргает красный светодиод)
-Если сетодиод горит непрерывно красным, то значит связь установлена с другим пультом.
-
-2. Проведите процедуру сопряжения пульта и приемника (![Binding](../notes/binding.md))
-
-Нет связи с полетным контроллером
---------------
-Если нет связи с полетным контроллером, то на экране монитора компьютера в окне Channel Monitor не будут отображаться изменения положения слайдеров при перемещении стиками пульта.
-
-![Нет связи с полетным контроллером](assets/notmoveslider.jpg)
-
-1. Зайдите в МЕНЮ (удерживаем нажатой кнопку “ОК”)
-2. Выберите меню “System setup” (Кнопки Up/Down - для навигации, кнопка “ОК” - подтверждение выбора
-3. Выберите “RX setup” > “PPM OUTPUT” >  “On”
-4. Сохраните изменения (удерживаем нажатой кнопку “CANCEL”)
-
-

docs/rasp1.md

@@ -1,22 +0,0 @@
-Raspberry Pi
-============
-
-Raspberry Pi означает «малиновый пирог». Этот свободно помещающийся в ладони одноплатный компьютер создан на базе мобильного микропроцессора ARM11. У него низкое потребление энергии – и он может работать даже от солнечных батарей.
-
-* Вес – 45 грамм,
-* тактовая частота 700 МГц;
-* есть графическое ядро в процессоре Broadcom BCM2835
-* оперативная память – 512 Мб;
-* есть USB -разъемы (один или два в зависимости от модели); м
-* наиболее применяемая модель мини-компьютера Raspberry Pi – модель В - это модель В на с поддержкой Ethernet.
-
-Сферы применения компьютера Raspberry Pi достаточно широки, ведь это всё-таки вполне полноценный компьютер. Если вам нужна машина для решения несложных задач, которые не требуют применения мощных ресурсов в плане вычисления, то вы смело можете подключать к устройству Raspberry Pi стандартные элементы компьютера: монитор; мышь; клавиатуру;
-
-Вообще, Raspberry Pi – очень популярная платформа, на которой можно реализовать множество проектов, таких как
-
-* сервер домашней автоматизации (или система «умный дом»)
-* сервер хранения данных (NAS)
-* домашний медиа-сервер
-* «мозговой центр» для автоматизированных станков или роботов
-
-Собственно, в последнем качестве мы и будем его использовать, благодаря возможности подключения его к автопилоту Pixhawk.

docs/ru/3g.md

@@ -1,6 +1,6 @@
 Использование внешнего 3G-модема
 ===
 
-Использование внешнего 3G-модема на Raspberry возможно с помощью пакета `sakis3g`.
+Использование внешнего 3G-модема на Raspberry возможно с помощью пакета [`sakis3g`](https://github.com/Trixarian/sakis3g-source).
 
 TODO

docs/ru/4in1.md

@@ -0,0 +1,30 @@
+# Подключение регуляторов 4in1
+
+## Распиновка платы реуляторов 4in1
+
+Одним цветом выделены соответствующие фазные провода (рис. 1a) и управляющий ими сигнал (рис. 1b).
+
+Например, оранжевый цвет -> нижний правый мотор -> S1 - оранжевый провода.
+
+## Распиновка полетного контроллера Pixracer
+
+На рис. 2a указана распиновка гребенки:
+
+* **SIGNAL** – подключение регуляторов. Каждый пин имет свой собственный сигнал. На 5 и 6 сигнал можно получать ШИМ сигнал (Например, можно подключить сервопривод).
+* **GND** – земля полетного контроллера. Единая шина на всех пинах GND (отмечены черным).
+* 1, 2, 3, 4 – порты для подключения ESC.
+* 1, 2 - порты расширения выходного ШИМ сигнала (настраиваются в QGroundControl, также могут использоваться для управления гексакоптером).
+
+    На рис. 2b указана нумерация моторов полетного контроллера Pixracer.
+
+* Стрелочка – ориентация полетного контроллера.
+* Черные M3, M4 – моторы, вращающиеся по часовой стрелке.
+* Красные M1, M2 – моторы, вращающиеся против часовой стрелке.
+
+## Иллюстрация подключения, исходя из текущей ориентации платы регуляторов 4in1
+
+Используя рис. 1a, 1b, 2a, 2b необходимо сопоставить каждому мотору свой сигнал управления и подключить в соотвествии с порядком нумерации моторов Pixracer.
+
+Например, мотор М3, вращающийся против часовой стрелки (верхний левый угол) управляется сигналом S4 (зеленый провод). Подключается в порт 3.
+
+![Подключение регуляторов 4in1](../assets/cl3_connectionESC4in1.jpg)

docs/ru/README.md

@@ -1,7 +1,7 @@
 Клевер
 ======
 
-<img src="assets/clever3.png" align="right" width="300px" alt="Клевер">
+<img src="../assets/clever3.png" align="right" width="300px" alt="Клевер">
 
 «Клевер» — это учебный конструктор программируемого квадрокоптера, состоящего из популярных открытых компонентов, а также набор необходимой документации и библиотек для работы с ним.
 
@@ -18,7 +18,7 @@
 Образ для Raspberry Pi
 ----------------------
 
-**Образ ОС** для RPi 3 с предустановленным и преднастроенным ПО можно скачать [здесь](microsd_images.html).
+**Образ ОС** для RPi 3 с предустановленным и преднастроенным ПО можно скачать [здесь](microsd_images.md).
 
 Образ включает в себя:
 
@@ -29,6 +29,6 @@
 * mavros
 * Набор ПО для работы с Клевером
 
-[Описание API](simple_offboard.html) для автономных полетов.
+[Описание API](simple_offboard.md) для автономных полетов.
 
 Исходный код сборщика образа и всего ПО можно найти на [GitHub](https://github.com/CopterExpress/clever).

docs/ru/SUMMARY.md

@@ -0,0 +1,63 @@
+# Содержание
+
+* [Введение](README.md)
+* [Глоссарий](gloss.md)
+* Сборка
+  * [Сборка Клевера 2](assemble_2.md)
+  * [Сборка Клевера 3](assemble_3.md)
+  * [Установка FPV](fpv.md)
+  * [Безопасность](safety.md)
+  * [Подключение регулятора 4 в 1](4in1.md)
+  * [Типы силовых разъемов](connectortypes.md)
+  * [Лужение](zap.md)
+  * [Техника безопасности по пайке](tb.md)
+  * [Проверка подключения](test_connection.md)
+  * [Неисправности радиоаппаратуры](radioerrors.md)
+  * [Подключение GPS](gps.md)
+* Настройка
+  * [Первоначальная настройка](setup.md)
+  * [Полетные режимы](modes.md)
+  * [Прошивка Pixhawk/Pixracer](firmware.md)
+  * [Параметры PX4](px4_parameters.md)
+  * [Настройка PID](calibratePID.md)
+* Работа с Raspberry Pi
+  * [Raspberry Pi](raspberry.md)
+  * [Образ для RPi](microsd_images.md)
+  * [Подключение RPi к Pixhawk](connection.md)
+  * [Подключение по Wi-Fi](wifi.md)
+  * [SSH-доступ](ssh.md)
+  * [Настройка сети RPi](network.md)
+  * [Работа с QGroundControl через Wi-Fi](gcs_bridge.md)
+  * [Пилотирование со смартфона](rc.md)
+  * [Интерфейс UART](uart.md)
+  * [Просмотр видеострима с камер](web_video_server.md)
+  * [Системы координат](frames.md)
+* Программирование
+  * [ROS](ros.md)
+  * [MAVROS](mavros.md)
+  * [Автономный полет в OFFBOARD](simple_offboard.md)
+  * [Навигация по ArUco-маркерам](aruco.md)
+  * [Автоматическая проверка](selfcheck.md)
+  * [Примеры кода](snippets.md)
+  * [Ориентация камеры](camera_frame.md)
+  * [Камера \(компьютерное зрение\)](camera.md)
+  * [Светодиодная лента](leds.md)
+  * [Визуализация с помощью rviz](rviz.md)
+  * [Ультразвуковой дальномер](sonar.md)
+  * [Работа с SITL](sitl.md)
+  * [Автозапуск ПО](autolaunch.md)
+  * [Взаимодействие с Arduino](arduino.md)
+  * [3G-модем](3g.md)
+* Проекты на базе Клевера
+  * [Шаровая защита коптера](shield.md)
+  * [CopterHack-2018](copterhack2018.md)
+  * [CopterHack-2017](copterhack2017.md)
+* Дополнительные материалы
+  * [Вклад в Клевер](contributing.md)
+  * [Прошивка ESC контроллеров с помощью Arduino](esc_firmware.md)
+  * [Протокол MAVLink](mavlink.md)
+  * [Работа с логами PX4](flight_logs.md)
+* Учебник
+  * [Теория и видеоуроки](lessons.md)
+  * [Учебно-методическое пособие](metod.md)
+  * [Контрольные и проверочные материалы](tests.md)

docs/ru/arduino.md

@@ -1,6 +1,6 @@
 # Управление коптером с Arduino
 
-Для взаимодействия с ROS-топиками и сервисами на Raspberry Pi можно использовать библиотеку [rosserial_arduino](http://wiki.ros.org/rosserial_arduino).
+Для взаимодействия с ROS-топиками и сервисами на Raspberry Pi можно использовать библиотеку [rosserial_arduino](http://wiki.ros.org/rosserial_arduino). Эта библиотека предустановлена на [образе для Raspberry Pi](microsd_images.md).
 
 Основной туториал по rosserial: http://wiki.ros.org/rosserial_arduino/Tutorials
 
@@ -8,16 +8,14 @@ Arudino необходимо установить на Клевер и подк
 
 ## Настройка Arduino IDE
 
-Для работы с ROS, Arduino необходимо понимать формат сообщений. Для этого на Clever необходимо собрать библиотеку ROS-сообщений (`ros_lib`) и скопировать в папку `<папку скетчей>/libraries`.
-
-Для сборки библиотеки на коптере необходимо выполнить следующий скрипт:
+Для работы с ROS Arduino необходимо понимать формат сообщений установленных пакетов. Для этого [на Raspberry Pi](ssh.md) необходимо собрать библиотеку ROS-сообщений:
 
 ```bash
 rosrun rosserial_arduino make_libraries.py .
-tar czf clever_arudino.tar.gz ros_lib
-rm -rf ros_lib
 ```
 
+Полученный каталог `ros_lib` необходимо скопировать в `<папку скетчей>/libraries` на компьютере с Arudino IDE.
+
 ## Настройка Raspberry Pi
 
 Чтобы единоразово запустить программу на Arduino, можно воспользоваться командой:
@@ -40,7 +38,7 @@ sudo systemctl restart clever
 
 ## Задержки
 
-При использовании `rosserial_arduino` микроконтроллер Arduino не должен быть заблокирован больше чем на несколько секунд (например, с использованием функции `delay`); иначе связь между Raspberry Pi и Arduino будет разорвана. 
+При использовании `rosserial_arduino` микроконтроллер Arduino не должен быть заблокирован больше чем на несколько секунд (например, с использованием функции `delay`); иначе связь между Raspberry Pi и Arduino будет разорвана.
 
 При реализации долгих циклов `while` обеспечьте периодический вызов функции `hn.spinOnce`:
 
@@ -121,7 +119,7 @@ void setup()
   	delay(1000);
   	nh.spinOnce();
   }
-    
+
   nav_req.auto_arm = false;
 
   // Пролет вперед на 3 метра:
@@ -133,13 +131,13 @@ void setup()
   nav_req.frame_id = "fcu_horiz";
   nav_req.speed = 0.8;
   navigate.call(nav_req, nav_res);
-  
+
   // Ждем 5 секунд
   for(int i=0; i<5; i++) {
     delay(1000);
     nh.spinOnce();
-  }  
-  
+  }
+
   // Полет в точку 1:0:2 по маркерному полю
   nh.loginfo("Fly on point");
   nav_req.auto_arm = false;
@@ -229,4 +227,4 @@ typedef ros::NodeHandle_<ArduinoHardware, 3, 3, 100, 100> NodeHandle;
 
 // ...
 NodeHandle nh;
-```
+```