docs: some info on optical flow troubleshooting

.gitignore

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

.markdownlint.json

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

.travis.yml

@@ -4,7 +4,7 @@ services:
   - docker
 env:
   global:
-    - DOCKER="goldarte/img-tool:builder-mod"
+    - DOCKER="smirart/img-tool:v0.1"
     - 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 "goldarte"
+  - git config --local user.name "urpylka"
-  - git config --local user.email "goldartt@gmail.com"
+  - git config --local user.email "urpylka@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/CopterExpress/clever.svg?branch=master)](https://travis-ci.org/CopterExpress/clever)
+[![Build Status](https://travis-ci.org/urpylka/clever.svg?branch=master)](https://travis-ci.org/urpylka/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://clever.copterexpress.com/simple_offboard.html).
+API description (in Russian) for autonomous flights is available [on GitBook](https://copterexpress.gitbooks.io/clever/simple_offboard.html).
 
 ## Manual installation
 

apps/ios/cleverrc/telemetry.js

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

aruco_pose/map/map.txt

@@ -0,0 +1,4 @@
+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

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

aruco_pose/msg/MarkerArray.msg

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

aruco_pose/msg/Point2D.msg

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

aruco_pose/nodelet_plugins.xml

@@ -1,5 +1,8 @@
 <library path="lib/libaruco_pose">
-   <class name="aruco_pose/aruco_pose" type="ArucoPose" base_class_type="nodelet::Nodelet">
+   <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">
       <description/>
    </class>
 </library>

aruco_pose/package.xml

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

aruco_pose/src/aruco_detect.cpp

@@ -0,0 +1,293 @@
+/*
+ * 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

@@ -0,0 +1,307 @@
+/*
+ * 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

@@ -1,350 +0,0 @@
-#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

@@ -1,145 +0,0 @@
-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

@@ -0,0 +1,22 @@
+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

@@ -1,20 +0,0 @@
-#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

@@ -0,0 +1,109 @@
+#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,21 +4,19 @@
     "author": "Copter Express",
     "language": "ru",
     "root": "docs/",
-    "plugins": [
+    "plugins": ["youtube", "richquotes", "versions", "yametrika"],
-        "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,7 +4,6 @@
 	<!-- <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,15 +84,6 @@ ${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~deb9u3 > /dev/null \
+&& apt-get install --no-install-recommends -y -qq dirmngr=2.1.18-8~deb9u2 > /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,14 +92,13 @@ libjpeg8-dev=8d1-2 \
 tcpdump \
 ltrace \
 libpoco-dev=1.7.6+dfsg1-5+deb9u1 \
-python-rosdep=0.13.0-1 \
+python-rosdep=0.12.2-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)
 
@@ -107,24 +106,15 @@ pigpio python-pigpio python3-pigpio \
 sed -i "s/updates_available//" /usr/share/byobu/status/status
 # sed -i "s/updates_available//" /home/pi/.byobu/status
 
-#echo_stamp "Upgrade pip"
+echo_stamp "Upgrade pip"
-#my_travis_retry pip install --upgrade pip
+my_travis_retry pip install --upgrade pip
-#my_travis_retry pip3 install --upgrade pip
+my_travis_retry pip3 install --upgrade pip3
-
-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,12 +167,16 @@ 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,24 +1,35 @@
 <launch>
-    <remap from="image" to="main_camera/image_raw"/>
+    <arg name="aruco_detect" default="true"/>
-    <remap from="camera_info" to="main_camera/camera_info"/>
+    <arg name="aruco_map" default="false"/>
+    <arg name="aruco_vpe" default="false"/>
 
-    <node pkg="nodelet" type="nodelet" name="aruco_pose" args="load aruco_pose/aruco_pose nodelet_manager" clear_params="true">
+    <!-- detect aruco markers, estimate poses -->
-        <param name="frame_id" value="aruco_map_raw"/>
+    <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">
-        <param name="type" value="gridboard"/>
+        <remap from="image_raw" to="main_camera/image_raw"/>
-        <param name="markers_x" value="1"/>
+        <remap from="camera_info" to="main_camera/camera_info"/>
-        <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"/>
 
-        <!-- Custom gridboard: -->
+        <param name="length" value="0.32"/>
-        <!--<rosparam param="marker_ids">[6, 5, 4, 3, 2, 1]</rosparam>-->
+        <param name="snap_orientation" value="local_origin"/>
+        <param name="estimate_poses" value="true"/>
     </node>
 
-    <node pkg="nodelet" type="nodelet" name="aruco_vpe" args="load clever/aruco_vpe nodelet_manager" clear_params="true">
+    <!-- estimate aruco map pose -->
-        <param name="aruco_orientation" value="local_origin"/>
+    <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">
-        <!--<param name="aruco_orientation" value="local_origin_upside_down"/>-->
+        <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>
 
-        <param name="use_mocap" value="true"/>
+    <!-- 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"/>
     </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 vpe -->
+    <!-- aruco -->
     <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="2"/>
+        <param name="num_worker_threads" value="4"/>
     </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,18 +2,13 @@
     <!-- 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 -->
 
-    <!-- article about camera setup: https://clever.copterexpress.com/camera_frame.html -->
+    <!-- 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"/>-->
 
-    <!-- camera is oriented downward, camera cable goes backward [option 1] -->
+    <!-- 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"/>
+    <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 forward  [option 2] -->
+    <!-- clever 3, upwards -->
-    <!--<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
-import tf.transformations as t
+from aruco_pose.msg import MarkerArray
 
 
 # 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,58 +66,37 @@ 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_pose/debug', Image, timeout=1)
+        rospy.wait_for_message('aruco_detect/markers', MarkerArray, timeout=0.5)
     except rospy.ROSException:
-        failure('no aruco_pose/debug messages')
+        failure('No aruco markers detection')
 
 
-@check('Vision position estimate')
+@check('Visual position estimate')
 def check_vpe():
     try:
-        vis = rospy.wait_for_message('mavros/vision_pose/pose', PoseStamped, timeout=1)
+        rospy.wait_for_message('mavros/vision_pose/pose', PoseStamped, timeout=1)
     except rospy.ROSException:
         try:
-            vis = rospy.wait_for_message('mavros/mocap/pose', PoseStamped, timeout=1)
+            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')
@@ -127,7 +106,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')
@@ -135,25 +114,15 @@ 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:
-        pose = rospy.wait_for_message('mavros/local_position/pose', PoseStamped, timeout=1)
+        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')
@@ -163,23 +132,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)')
@@ -187,7 +156,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')
@@ -196,7 +165,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')
@@ -205,11 +174,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')
@@ -237,7 +206,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

@@ -0,0 +1,36 @@
+#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

@@ -0,0 +1,99 @@
+/*
+ * 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/3g.md

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

docs/LANGS.md

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

docs/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.md).
+**Образ ОС** для RPi 3 с предустановленным и преднастроенным ПО можно скачать [здесь](microsd_images.html).
 
 Образ включает в себя:
 
@@ -29,6 +29,6 @@
 * mavros
 * Набор ПО для работы с Клевером
 
-[Описание API](simple_offboard.md) для автономных полетов.
+[Описание API](simple_offboard.html) для автономных полетов.
 
 Исходный код сборщика образа и всего ПО можно найти на [GitHub](https://github.com/CopterExpress/clever).

docs/SUMMARY.md

@@ -0,0 +1,54 @@
+# 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/arduino.md

@@ -1,6 +1,6 @@
 # Управление коптером с Arduino
 
-Для взаимодействия с ROS-топиками и сервисами на Raspberry Pi можно использовать библиотеку [rosserial_arduino](http://wiki.ros.org/rosserial_arduino). Эта библиотека предустановлена на [образе для Raspberry Pi](microsd_images.md).
+Для взаимодействия с ROS-топиками и сервисами на Raspberry Pi можно использовать библиотеку [rosserial_arduino](http://wiki.ros.org/rosserial_arduino).
 
 Основной туториал по rosserial: http://wiki.ros.org/rosserial_arduino/Tutorials
 
@@ -8,14 +8,16 @@ Arudino необходимо установить на Клевер и подк
 
 ## Настройка Arduino IDE
 
-Для работы с ROS Arduino необходимо понимать формат сообщений установленных пакетов. Для этого [на Raspberry Pi](ssh.md) необходимо собрать библиотеку ROS-сообщений:
+Для работы с ROS, Arduino необходимо понимать формат сообщений. Для этого на Clever необходимо собрать библиотеку ROS-сообщений (`ros_lib`) и скопировать в папку `<папку скетчей>/libraries`.
+
+Для сборки библиотеки на коптере необходимо выполнить следующий скрипт:
 
 ```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, можно воспользоваться командой:

docs/aruco.md

@@ -5,7 +5,7 @@
 
 Пример ArUco-маркеров:
 
-![ArUco-маркеры](../assets/markers.jpg)
+![](assets/markers.jpg)
 
 > **Hint** При печати визуальных маркеров необходимо использовать максимально матовую бумагу. Глянцевая бумага будет бликовать на свету, сильно ухудшая качество распознавания.
 
@@ -39,7 +39,7 @@ sudo systemctl restart clever
 
 В качестве карты меток можно использовать автоматически сгенерированный [ArUco-board](https://docs.opencv.org/trunk/db/da9/tutorial_aruco_board_detection.html).
 
-Настройка карты меток производится с помощью файла `~/catkin_ws/src/clever/clever/launch/aruco.launch`. Для использования ArUco-board введите его параметры:
+Настройка карты меток производится с помощью файла `~/catkin_ws/src/clever/clever/aruco.launch`. Для использования AruCo-board введите его параметры:
 
 ```xml
 <node pkg="nodelet" type="nodelet" name="aruco_pose" args="load aruco_pose/aruco_pose nodelet_manager">
@@ -84,7 +84,7 @@ sudo systemctl restart clever
 
 Для контроля карты, по которой в данный момент коптер осуществляет навигацию, можно просмотреть содержимое топика `aruco_pose/map_image`. Через браузер его можно просмотреть при помощи [web\_video\_server](web_video_server.md) по ссылке [http://192.168.11.1:8080/snapshot?topic=/aruco\_pose/map\_image](http://192.168.11.1:8080/snapshot?topic=/aruco_pose/map_image):
 
-![](../../assets/Снимок экрана 2017-11-27 в 23.20.49.png)
+![](assets/Снимок экрана 2017-11-27 в 23.20.49.png)
 
 При полетах необходимо убедиться, что наклеенные на пол метки соответствуют карте.
 
@@ -104,18 +104,14 @@ _Примечание_: указанное выше определение пр
 
 Таким образом, нулевой является левая нижня точка маркерного поля. Угол по рысканью считается равным 0, когда коптер смотрит направо \(по оси x\).
 
-![Система координат макеров](../assets/aruco-frame.png)
+![](assets/aruco-frame.png)
 
 ### Настройка полетного контролера
 
 Для правильной работы Vision Position Estimation необходимо \(через [QGroundControl](gcs_bridge.md)\) убедиться, что:
 
-* **Для Pixhawk**: Установлена прошивка с LPE \(local position estimator\). Для Pixhawk необходимо [скачать прошивку `px4fmu-v2_lpe.px4`](https://github.com/PX4/Firmware/releases).
+* Для Pixhawk: Установлена прошивка с LPE \(local position estimator\). Для Pixhawk необходимо [скачать прошивку `px4fmu-v2_lpe.px4`](https://github.com/PX4/Firmware/releases). Для Pixracer параметр `SYS_MC_EST_GROUP` должен быть установлен в `local_position_estimator, attitude_estimator_q`.
-
+* В параметре `LPE_FUSION` включены **только** флажки `vision position`, `vision yaw`, `land detector`. Итоговое значение _28_.
-  **Для Pixracer**: параметр `SYS_MC_EST_GROUP` должен быть установлен в `local_position_estimator, attitude_estimator_q`.
-
-  > **Note** После изменения значения параметра `SYS_MC_EST_GROUP` необходимо перезагрузить полетный контроллер.
-* В параметре `LPE_FUSION` включены **только** флажки `vision position`, `land detector`. Итоговое значение _20_.
 * Выключен компас: `ATT_W_MAG` = 0
 * Вес угла по рысканью по зрению: `ATT_W_EXT_HDG` = 0.5
 * Включена ориентация по Yaw по зрению: `ATT_EXT_HDG_M` = 2 `MOCAP`.
@@ -126,11 +122,9 @@ _Примечание_: указанное выше определение пр
   * `LNDMC_THR_RANGE` = 0.5
   * `LNDMC_Z_VEL_MAX` = 1 m/s
 
-<!--
 Для простоты настройки можно воспользоваться готовым файлом настроек для [Clever 2](https://github.com/CopterExpress/clever/blob/master/docs/assets/Clever2LPE_160118.params) или для [Clever 3](https://github.com/CopterExpress/clever/blob/master/docs/assets/Clever3_LPE_020218.params) и вгрузить его в контроллер с помощью меню Tools - Load from file из раздела Parameters в QGroundControl.
 
-![](../assets/Screenshot from 2018-02-27 22-30-50.png)
+![](assets/Screenshot from 2018-02-27 22-30-50.png)
--->
 
 ### Полет
 
@@ -149,11 +143,13 @@ time.sleep(5)
 navigate(2, 2, 2, speed=1, frame_id='aruco_map', update_frame=True)  #  полет в координату 2:2, высота 3 метра
 ```
 
-См. [другие функции](simple_offboard.md) simple_offboard.
+См. [другие функции](simple_offboard.md) simple offboard.
 
 ### Расположение маркеров на потолке
 
-![Маркеры на потолке](../assets/IMG_4175.JPG)
+> **Info** Образ версии &gt;0.2.
+
+![](assets/IMG_4175.JPG)
 
 Для навигации по маркерам, расположенным на потолке, необходимо поставить основную камеру так, чтобы она смотрела вверх и [установить соответствующий фрейм камеры](camera_frame.md).
 
@@ -168,3 +164,6 @@ navigate(2, 2, 2, speed=1, frame_id='aruco_map', update_frame=True)  #  поле
 ```python
 navigate(x=1, y=2, z=1.1, speed=0.5, frame_id='aruco_map')
 ```
+
+
+

docs/assemble.md

@@ -1,11 +1,10 @@
-Инструкция по сборке конструктора Клевер 2
+# Инструкция по сборке конструктора Клевер 2
-============================================
 
-![Clever](../assets/clever2.jpg)
+![Clever](assets/Clevermain.png)
 
 ## Состав конструктора
 
-![Explosion](../assets/explosion.jpg)
+![Explosion](assets/explosion.png)
 
 * Рама центральная x2.
 * Рама дополнительная х4.
@@ -26,7 +25,7 @@
 * Зарядное устройство EFEST Luc V4 Li-lon x1.
 * Защитный бокс регуляторов x4.
 * Крепление под ножки x8.
-* Полетный контроллер Pixhawk x1.
+* Полетный контроллер PixHawk x1.
 * Радиоприемник FlySky i6 x1.
 * Радиопульт FlySky i6 x1.
 * Зарядное устройство EFEST LUC V4 x1.
@@ -74,7 +73,7 @@
 16. Ручка A (VrA).
 17. Ручка B (VrB).
 
-![radio Transmitter](../assets/radioTransmitter.png)
+![radio Transmitter](assets/radioTransmitter.png)
 
 ## Дополнительное оборудование
 
@@ -89,7 +88,7 @@
 7. Канцелярский нож
 8. Мультиметр
 
-![Дополнительное оборудование](../assets/addEqipment.jpg)
+![Дополнительное оборудование](assets/addEqipment.jpg)
 
 [Техника безопасности при пайке](tb.md)
 
@@ -99,7 +98,7 @@
 
 * Распаковать моторы. Используя плоскогубцы, укоротить провода на моторах, обрезать половину длины (оставив 25 мм).
 
-![Мотор brrc2205](../assets/brrc2205.png)
+![Мотор brrc2205](assets/brrc2205.png)
 
 Зачистить
 
@@ -112,18 +111,18 @@
 * Нанести флюс на оголенную часть провода.
 * Покрыть припоем, используя пинцет.
 
-![Лужение](../assets/zap.jpg)
+![Лужение](assets/zap.jpg)
 
 #### Закрепить мотор на луче
 
 * Установить мотор на сторону луча с гравировкой.
 * Прикрепить моторы к лучам винтами М3х8, используя отвертку.
 
-![Закрепить мотор на луче](../assets/brrc2205on.jpg)
+![Закрепить мотор на луче](assets/brrc2205on.png)
 
 * Лучи с моторами необходимо расположить согласно схеме. Стрелками указано направление вращения моторов.
 
-![Вращение моторов](../assets/brrc2205ondeck.jpg)
+![Вращение моторов](assets/brrc2205ondeck.png)
 
 ### Залудить три контактные площадки регулятора
 
@@ -132,7 +131,7 @@
 
 Чтобы припой аккуратно заполнил всю площадку, необходимо прогреть площадку регулятора. Для этого нужно удерживать жало паяльника на контактной плащадке в течение 2 сек (или больше, если потребуется)
 
-![Лужение контактных площадок регуляторов](../assets/escDYSzap.png)
+![Лужение контактных площадок регуляторов](assets/escDYSzap.png)
 
 * Повторить данную операцию для оставшихся трех регуляторов
 
@@ -140,12 +139,14 @@
 
 Припаять ранее приготовленные провода моторов к контактным площадкам регуляторов.
 
-![Припаять провода моторов к регуляторам](../assets/solderingBrrc2205ondeckTOescDYSzap.png)
+![Припаять провода моторов к регуляторам](assets/solderingBrrc2205ondeckTOescDYSzap.png)
 
 * Повторить данную операцию для оставшихся трех регуляторов
 
 ### Монтаж разъемов питания
 
+[Статья про силовые и управляющие цепи](powerConnection.md)
+
 #### Подготовка проводов для силовых разъемов XT60
 
 1. Взять моток красных и черных проводов, промаркированных как 14AWG
@@ -153,13 +154,13 @@
 * Длина 7 см (Для силового разъема XT60 pin) - 1 красный, 1 черный
 * Длина 9 см (Для силового разъема XT60 socket) - 1 красный, 1 черный
 
-![Подготовка проводов для силового разъема](../assets/cutwire14AWG.jpg)
+![Подготовка проводов для силового разъема](assets/cutwire14AWG.jpg)
 
 #### Подготовка силовых разъемов питания XT60 pin и XT60 socket
 
 [Статья про силовые разъемы и их обозначения](connectortypes.md)
 
-![Силовой разъем XT60](../assets/xt60pinsocket.jpg)
+![Силовой разъем XT60](assets/xt60pinsocket.jpg)
 
 1. Под разъем XT60 pin залудить два силовых провода красный и чёрный 14AWG длиной 7 см.
 2. Залудить контактные площадки разъема XT60 pin.
@@ -167,7 +168,7 @@
 4. Припаять красный провод к “+” контакту разъема .
 5. Нарезать термоусадку ф5 (2 отрезка по 10 мм).
 6. Надеть термоусадку ф5 на провода так, чтобы она закрывала контактные площадки проводов с XT60 .
-7. Усадить термоусадку феном. ![Монтаж разъемов XT60](../assets/mountxt60pinsocket.png)
+7. Усадить термоусадку феном. ![Монтаж разъемов XT60](assets/mountxt60pinsocket.png)
 8. Повторить процедуру для разъема XT60 socket.
 
 #### Подготовка разъема питания управляющей цепи 5В
@@ -177,15 +178,15 @@
 3. Убрать 3-й (оранжевый) провод из разъема, за ненадобностью.
 4. Длина оставшихся черного и красного проводов  10-12 см.
 
-![Монтаж разъема 5В](../assets/mount5vconnector.jpg)
+![Монтаж разъема 5В](assets/mount5vconnector.png) *было бы круто, если делать такие картинки и в формате гифки
 
 ### Монтаж платы распределения питания
 
 #### Предпаячная проверка
 
-[Статья про прозвонку](test_connection.md)
+[Статья про прозвонку](testConnection.md)
 
-![Предпаячная проверка](../assets/startPDBtest.jpg)
+![Предпаячная проверка](assets/startPDBtest.jpg)
 
 Прозвонить следующие цепи на НЕЗАМКНУТОСТЬ (отсутствие звукового сигнала мультиметра):
 
@@ -203,7 +204,7 @@
 1. [Залудить*](zap.md) контактные площадки платы питания.
 2. С помощью мультиметра проверить отсутствие контактного замыкания на плате (прозвонить)
 
-![Постпаячная проверка](../assets/zapPDBtest.jpg)
+![Постпаячная проверка](assets/zapPDBtest.jpg)
 
 Чтобы припой аккуратно заполнил всю площадку, необходимо её прогреть. Для этого нужно удерживать жало паяльника на контактной плащадке в течение 2 сек (или больше, если потребуется)
 
@@ -211,7 +212,7 @@
 
 Припаять разъем для АКБ, соблюдая полярность на контактных площадках.
 
-![Пайка XT60 на PDB](../assets/solderingxt60socketTOpdb.jpg)
+![Пайка XT60 на PDB](assets/solderingxt60socketTOpdb.png)
 
 ВАЖНО о полярности
 
@@ -223,13 +224,13 @@
 Припаять разъем 5В, соблюдая полярность на контактных площадках.
 (на изображении: красный провод - это питание “+”)
 
-![Пайка 5В на PDB](../assets/soldering5VTOpdb.jpg)
+![Пайка 5В на PDB](assets/soldering5VTOpdb.png)
 
 ### Монтаж отсека АКБ
 
 #### Подготовка перемычек (3 шт.)
 
-![Перемычка](../assets/jumper.png)
+![Перемычка](assets/jumper.png)
 
 * Отрезать силовой провод длиной 2 см.
 * Зачистить с обеих сторон.
@@ -240,31 +241,31 @@
 
 #### Подготовка отсека АКБ
 
-![Подготовка отсека АКБ](../assets/casebattery.jpg)
+![Подготовка отсека АКБ](assets/casebattery.png)
 
 * Приклеить наклейки с разметкой внутрь отсека АКБ, в соответствии с полярностью.
 * Приклеить ленту из скотча на дно отсека.
 
 ### Монтаж платы распределения питания
 
-* Установить плату питания на раму винтами М3х8 и пластиковыми гайками. ![Установка платы PDB](../assets/mountPDB.png)
+* Установить плату питания на раму винтами М3х8 и пластиковыми гайками. ![Установка платы PDB](assets/mountPDB.png)
     > **ВАЖНО** Стрелочка на плате направлена в сторону носового выреза
-![Установка платы PDB](../assets/topviewmountPDB.png)
+![Установка платы PDB](assets/topviewmountPDB.png)
 
 #### Монтаж элементов
 
-1. Установить гайки в пластиковые держатели. ![Монтаж пластиковых держателей](../assets/holderLegs.png)
+1. Установить гайки в пластиковые держатели. ![Монтаж пластиковых держателей](assets/holderLegs.png)
 2. Установить лучи на раму винтами М3х16
     * Лучи устанавливаются поверх рамы
-    * Пластиковые держатели устанавливаются снизу рамы. ![Монтаж лучей](../assets/mountBeams.png)
+    * Пластиковые держатели устанавливаются снизу рамы. ![Монтаж лучей](assets/mountBeams.png)
-3. Расположение моторов. Проверить расположение моторов (моторы с черной гайкой в левом верхнем углу и в правом нижнем). ![Расположение моторов](../assets/motorsTopview.png)
+3. Расположение моторов. Проверить расположение моторов (моторы с черной гайкой в левом верхнем углу и в правом нижнем). ![Расположение моторов](assets/motorsTopview.png)
-4. Продеть силовые провода регуляторов в отверстия. ![силовые провода моторов](../assets/escWires.png)
+4. Продеть силовые провода регуляторов в отверстия. ![силовые провода моторов](assets/escWires.png)
 
 #### Пайка силовой цепи платы питания
 
 Припаять силовые провода регуляторов к плате питания, соблюдая полярность.
 
-![Пайка силовых проводов на PDB](../assets/solderingPowerwires.png)
+![Пайка силовых проводов на PDB](assets/solderingPowerwires.png)
 
 ВАЖНО о полярности
 
@@ -273,12 +274,12 @@
 
 ### Сопряжение приемника и пульта
 
-1. Подключить радиоприемник к разъему 5В. В любой разъем, GND внизу. На схеме питание обозначено как 5V ![Подключение питания приемника](../assets/receiver5V.png)
+1. Подключить радиоприемник к разъему 5В. В любой разъем, GND внизу. На схеме питание обозначено как 5V ![Подключение питания приемника](assets/receiver5V.png)
 2. Подключить АКБ. Светодиод на радиоприемнике должен мигать. ![Подключение АКБ]
 
 #### БЕЗОПАСНОСТЬ при работе с АКБ
 
-![БЕЗОПАСНОСТЬ при работе с АКБ](../assets/safetyPower.png)
+![БЕЗОПАСНОСТЬ при работе с АКБ](assets/safetyPower.png)
 
 #### Включение радиопульта
 
@@ -290,45 +291,45 @@
 6. Отсоединить джампер.
 7. Светодиод горит непрерывно.
 
-![Подключение питания приемника](../assets/connectingRadio.jpg)
+![Подключение питания приемника](assets/connectingRadio.png)
 
-[Мануал по неисправностям радиоаппаратуры](radioerrors.md)
+[Мануал по неисправностям радиоаппаратуры](radioerrors1.md)
 
 ### Проверка направления вращения моторов
 
 1. Наклеить наклейки на АКБ 18650.
-2. Установить 18650 в отсек АКБ, соблюдая полярность. ![Готовность отсека АКБ](../assets/readyBatteryholder.png)
+2. Установить 18650 в отсек АКБ, соблюдая полярность. ![Готовность отсека АКБ](assets/readyBatteryholder.png)
 3. Проверить, что разъем питания 5В подключен к приемнику по схеме.
-4. Подключить регулятор мотора к 3 каналу приемника CH3 по схеме. ![Подключение регулятора к приемнику](../assets/connectionESCtoReceiver.png)
+4. Подключить регулятор мотора к 3 каналу приемника CH3 по схеме. ![Подключение регулятора к приемнику](assets/connectionESCtoReceiver.png)
 5. Подключить внешнее питание (АКБ).
 6. Включить пульт.
 7. Подать левым стиком газ (throttle) на 10%.
-8. Проверить направления вращения мотора по схеме. ![Проверка вращения моторов](../assets/testMotors.jpg)
+8. Проверить направления вращения мотора по схеме. ![Проверка вращения моторов](assets/testMotors.png)
-9. Если необходимо изменить направление вращения, то меняем любые два фазных провода мотора (нужно перепаять). ![Перепайка фазных проводов](../assets/resolderingESC.png)
+9. Если необходимо изменить направление вращения, то меняем любые два фазных провода мотора (нужно перепаять). ![Перепайка фазных проводов](assets/resolderingESC.png)
 
 ### Монтаж радиоприемника
 
 1. Установить пластиковые стойки 30 мм на раму винтами М3х8.
-2. Разъем питания 5В продеть в прорезь. ![Установка стоек и прорезь](../assets/mountReceiverStud.png)
+2. Разъем питания 5В продеть в прорезь. ![Установка стоек и прорезь](assets/mountReceiverStud.png)
-3. Приемник прикрепить к нижней дополнительной раме, используя двухсторонний скотч и ориентируясь на гравировку. Антенны направлены вперед. ![Установка радиоприемника на деку](../assets/mountReceiverDeck.png)
+3. Приемник прикрепить к нижней дополнительной раме, используя двухсторонний скотч и ориентируясь на гравировку. Антенны направлены вперед. ![Установка радиоприемника на деку](assets/mountReceiverDeck.png)
-4. Установить 3х проводной шлейф в канал PPM / CH1. ![Подключение радиоприемника](../assets/receiverPPM.png)
+4. Установить 3х проводной шлейф в канал PPM / CH1. ![Подключение радиоприемника](assets/receiverPPM.png)
 5. Продеть в прорезь к разъему 5 В.
-6. Прикрутить нижнюю дополнительную раму к стойкам на центральной раме винтами М3х8. ![Установка нижней деки](../assets/mountBottomDeck.png)
+6. Прикрутить нижнюю дополнительную раму к стойкам на центральной раме винтами М3х8. ![Установка нижней деки](assets/mountBottomDeck.png)
     > **ВАЖНО** Направление стрелок на плате питания и на дополнительной раме совпадают
 
 ### Монтаж полетного контроллера
 
 #### Переворачиваем сборку
 
-![Переворачиваем сборку](../assets/topPreview.png)
+![Переворачиваем сборку](assets/topPreview.png)
 
-#### Установка полетного контроллера Pixhawk
+#### Установка полетного контроллера PixHawk
 
-1. Клеим 2х сторонний скотч по углам полетного контроллера. ![Полетный контроллер](../assets/pixhawk.png)
+1. Клеим 2х сторонний скотч по углам полетного контроллера. ![Полетный контроллер](assets/pixhawk.png)
-    > **ВАЖНО** При работе моторов возникают вибрации, отрицательно влияющие на показания датчиков полетного контроллера Pixhawk. Чтобы избежать этого эффекта, количество слоев двустороннего скотча
+    > **ВАЖНО** При работе моторов возникают вибрации, отрицательно влияющие на показания датчиков полетного контроллера PixHawk. Чтобы избежать этого эффекта, количество слоев двустороннего скотча
 лучше увеличить до 4-5.
-2. Установить  полетный контроллер в центр рамы. ![Полетный контроллер](../assets/topviewpixhawk.jpg)
+2. Установить  полетный контроллер в центр рамы. ![Полетный контроллер](assets/topviewpixhawk.png)
-    > **ВАЖНО** Стрелки на раме и Pixhawk должны быть сонаправлены
+    > **ВАЖНО** Стрелки на раме и PixHawk должны быть сонаправлены
 
 #### Подключение полетного контроллера по схеме
 
@@ -336,19 +337,19 @@
 2. Моторы к 1,2,3,4 портам MAIN OUT, согласно схеме
 3. Питание от PDB (5В/VCC) в любой порт, кроме SB (SBUS)
 
- ![Подключение полетного контроллера](../assets/connectionPixhawk.png)
+ ![Подключение полетного контроллера](assets/connectionPixhawk.png)
 
 ### Сборка регуляторов
 
-1. Клеим 2х сторонний скотч на основание защитного бокса регуляторов. ![Скотч на бокс регулей](../assets/escCase.png)
+1. Клеим 2х сторонний скотч на основание защитного бокса регуляторов. ![Скотч на бокс регулей](assets/escCase.png)
-2. Укладываем регуляторы в защитные боксы. Крепим полученную сборку к лучам рамы. ![Вид сверху с боксами для  регулей](../assets/topESCcaseview.png)
+2. Укладываем регуляторы в защитные боксы. Крепим полученную сборку к лучам рамы. ![Вид сверху с боксами для  регулей](assets/topESCcaseview.png)
 
 ### Установка защиты
 
-1. Закрепить нижнюю защиту винтами М3х16 на лучах рамы. ![Установка лучевой защиты](../assets/lowsafeDeck.png)
+1. Закрепить нижнюю защиту винтами М3х16 на лучах рамы. ![Установка лучевой защиты](assets/lowsafeDeck.png)
-2. Закрепить ножки к пластиковым держателям винтами М3х16. ![Установка ножек](../assets/safeLegs.png)
+2. Закрепить ножки к пластиковым держателям винтами М3х16. ![Установка ножек](assets/safeLegs.png)
-3. Закрепить стойки 30 мм в отверстия нижней защиты винтами М3х12. ![Установка нижней радиальной защиты](../assets/safelowRadial.png)
+3. Закрепить стойки 30 мм в отверстия нижней защиты винтами М3х12. ![Установка нижней радиальной защиты](assets/safelowRadial.png)
-4. Закрепить верхнюю защиту винтами М3х12. ![Установка верхней радиальной защиты](../assets/safehighRadial.png)
+4. Закрепить верхнюю защиту винтами М3х12. ![Установка верхней радиальной защиты](assets/safehighRadial.png)
 
 ### Монтаж отсека АКБ
 
@@ -359,15 +360,15 @@
 * Рама дополнительная (1 шт)
 * Батарейный отсек (1 шт)
 
-1. Прикрепить батарейный отсек на верхнюю дополнительную раму винтами М3х12 и гайками. ![Монтаж отсека АКБ](../assets/mountHolder.png)
+1. Прикрепить батарейный отсек на верхнюю дополнительную раму винтами М3х12 и гайками. ![Монтаж отсека АКБ](assets/mountHolder.png)
-2. Прикрепить верхнюю дополнительную раму на стойки винтами М3х8. ![Монтаж отсека АКБ](../assets/isoViewmountHolder.jpg)
+2. Прикрепить верхнюю дополнительную раму на стойки винтами М3х8. ![Монтаж отсека АКБ](assets/isoViewmountHolder.png)
 3. Установить АКБ в отсек.
 
 ### Монтаж антенн
 
 1. Крепим антенны на 2х сторонний скотч или изоленту, а усики продеваем в передние отверстия верхней дополнительной рамы.
 
-![Монтаж отсека АКБ](../assets/mountAntenna.png)
+![Монтаж отсека АКБ](assets/mountAntenna.png)
 
 Коптер готов к настройке!
 
@@ -377,7 +378,7 @@
 2. Отключить аккумулятор. Держать питание выключенным. “Сборку, настройку и ремонт производить с отключенным питанием. Подключать питание только для тестирования электронных компонентов коптера. После тестирования перед другими работами питание сразу отключить.”
 3. Позвать на помощь. “Если при выполнении работ возникли какие-либо проблемы, необходимо обратиться к преподавателю или учителю, а не пытаться решить проблему самостоятельно.”
 
-![Безопасность при сборке](../assets/safetybyassem.png)
+![Безопасность при сборке](assets/safetybyassem.png)
 
 ## Безопасность при работе с Li-ion аккумуляторами 18650
 
@@ -385,4 +386,4 @@
 2. При подключении (отключении) аккумуляторов держаться только за разъёмы, тянуть или дергать за провода запрещается.
 3. В случае обрыва разъемов, обнаружения нарушений целостности изоляции или корпуса аккумулятора, не трогая его, немедленно сообщить преподавателю.
 
-См. статью [техника безопасности при пайке и лётной эксплуатации коптеров](safety.md).
+[ТЕХНИКА БЕЗОПАСНОСТИ ПРИ ПАЙКЕ И ЛЁТНОЙ ЭКСПЛУАТАЦИИ КОПТЕРОВ](safety.md)

docs/assemble_clever3_4in1.md

@@ -0,0 +1,154 @@
+# Инструкция по сборке конструктора Клевер 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/autolaunch.md

@@ -1,4 +1,4 @@
-Автозапуск ПО
+Автозапускаемое ПО
 ===
 
 systemd

docs/bundle.md

@@ -0,0 +1,31 @@
+Пакет программ на 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/calibratePID.md

@@ -1,16 +1,17 @@
-# Настройка коэффициентов PID
+### Как настраивать PID
 
-На практике самая распространенная проблема это быстрые осцилляции, возникающие из-за слишком большого значения параметра P. В данной ситуации следует уменьшить его значение (все параметры выставляются экспериментальным путем, исходя из поведения аппарата).
 
-Также стоит проверить чтобы осцилляций не было при резком спуске (в противном случае уменьшить P).
+На практике самая распространенная проблема это быстрые осцилляции, возникающие из-за слишком большого значения параметра P. 
+В данной ситуации следует уменьшить его значение (все параметры выставляются экспериментальным путем, исходя из поведения аппарата). 
+
+Также стоит проверить чтобы осцилляций не было при резком спуске (в противном случае уменьшить P)
 Медленные раскачивания коптера из стороны в сторону при попытке удержания заданной точки связаны с перебором значения I.
-В случае если при движении коптер раскачивается следует поднять это значение.
+В случае если при движении коптер раскачивается следует поднять это значение
-В случае если коптер плохо держит заданное положение следует увеличить параметр D при переборе или недостатке параметра D возникают осцилляции.
+В случае если коптер плохо держит заданное положение следует увеличить параметр D при переборе или недостатке параметра D возникают осцилляции
 
-> **Note** Настройку D следует начинать с минимальных значений, в 3–4 раза меньше значений по умолчанию, если таковые присутствуют.
+#### ВАЖНО настройку D следует начинать с минимальных значений , в 3-4 раза меньше значений по умолчанию, если таковые присутствуют
 
-Параметры для Rate Pitch and  Rate Roll должны быть одинаковыми.
+Параметры для Rate Pitch and  Rate Roll должны быть одинаковыми
+По YAW параметры следует менять отдельно, согласно вышеуказанной инструкции (как правило рысканье не требует серьезной настройки, можно оставить по умолчанию)
 
-По YAW параметры следует менять отдельно, согласно вышеуказанной инструкции (как правило рысканье не требует серьезной настройки, можно оставить по умолчанию).
+![Осцилляции по ROLL](assets/oscillRoll.jpg)
-
-![Осцилляции по ROLL](../assets/oscillRoll.jpg)

docs/camera_frame.md

@@ -8,7 +8,7 @@
 
 Эта строка задает статическую трансформацию между фреймом `fcu` ([соответствует корпусу полетного контроллера](frames.md)) и камерой (`main_camera_optical`) в формате:
 
-```txt
+```
 сдвиг_x сдвиг_y сдвиг_z угол_рысканье угол_тангаж угол_крен
 ```
 
@@ -22,40 +22,20 @@
 
 ## Настройки для Клевера
 
-> Первое изображение - как выглядит модель коптера в Rviz при указанных настройках, второе - как выглядит Клевер при тех же настройках.
+### Клевер 3, камера вниз
-
-### 1. Камера направлена вниз, шлейф назад
-
-```xml
-<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"/>
-```
-
-![](../assets/camera_option_1_rviz.png)
-![](../assets/camera_option_1_clever.jpg)
-
-### 2. Камера направлена вниз, шлейф вперёд
 
 ```xml
 <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"/>
 ```
 
-![](../assets/camera_option_2_rviz.png)
+### Клевер 3, камера вверх
-![](../assets/camera_option_2_clever.jpg)
-
-### 3. Камера направлена вверх, шлейф назад
-
-```xml
-<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"/>
-```
-
-![](../assets/camera_option_3_rviz.png)
-![](../assets/camera_option_3_clever.jpg)
-
-### 4. Камера направлена вверх, шлейф вперёд
 
 ```xml
 <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"/>
 ```
 
-![](../assets/camera_option_4_rviz.png)
+### Клевер 2, камера вниз
-![](../assets/camera_option_4_clever.jpg)
+
+```xml
+<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"/>
+```

docs/connection.md

@@ -1,9 +1,9 @@
 Подключение Pixhawk/Pixracer к Raspberry Pi
 ===
 
-Для программирования [автономных полетов](simple_offboard.md), [работы с Pixhawk (Pixracer) по Wi-Fi](gcs_bridge.md), использования [телефонного пульта](rc.md) и других функций необходимо подсоединить Raspberry Pi к Pixhawk (Pixracer).
+Для программирования [автономных полетов](simple_offboard.md), [работы с Pixhawk по Wi-Fi](gcs_bridge.md), использования [веб-пульта](web_rc.md) и других функций необходимо подсоединить Raspberry Pi к Pixhawk.
 
-Убедиться в работоспособности подключения, [выполнив на Raspberry Pi](ssh.md):
+Убедиться в работоспособности подключения, выполнив на Raspberry Pi:
 
 ```bash
 rostopic echo /mavros/state
@@ -28,7 +28,7 @@ rostopic echo /mavros/state
 sudo systemctl restart clever
 ```
 
-> **Hint** Для корректной работы подключения Raspberry Pi и Pixhawk по USB необходимо установить значение [параметра](px4_parameters.md) `CBRK_USB_CHK` на 197848.
+> **Hint** Для корректной работы подключения Raspberry Pi и Pixhawk по USB необходимо установить значение параметра `CBRK_USB_CHK` на 197848.
 
 Подключение по UART
 ---

docs/connectortypes.md

@@ -1,28 +1,27 @@
-# Типы силовых разъемов
+Типы силовых разъемов
-
+=====================
-## XT-60
+XT-60
-
+-----
 Один из самых надёжных силовых разъёмов, которые стараются применять на силовых аккумуляторах. Именно такие аккумуляторы используются на Li-Po аккумуляторах для коптеров. 
 
-<img src="../assets/xt60.jpg" alt="XT-60" width=200>
+![XT-60](assets/xt60.jpg)
-
-## T-plug
 
+T-plug
+------
 Аналог XT-60. Имеет различные вариации для упрозщения разъединения. 
 
-<img src="../assets/t-plug.jpg" alt="T-plug" width=200>
+![T-plug](assets/t-plug.jpg)
-
-## JST-XH или балансировочный разъем
 
+JST-XH или балансировочный разъем 
+ -----------------------------
 Разъёмы данного типа часто применяются для балансировки отдельных элементов в составе сборки из нескольких литий-полимерных (Li-Pol), литий-ионных (Li-ion) или литий-фосфатных (LiFePO4) аккумуляторов. 
 Подобные разъёмы с разным количеством штырьков устанавливаются в большинство современных зарядных устройств для балансировки литиевых элементов при заряде.
 Может использоваться в сочетании с Buzzer (пищалкой) для контроля заряда аккумулятора.
 
-<img src="../assets/balance.jpg" alt="JST-XH" width=200>
+![JST-XH](assets/balance.jpg)
-
-## Gold bullet Conector или "Бананы"
 
+ Gold bullet Conector или "Бананы"
+ -----------------------------
 Существует великое множество штырьковых разъёмов типа Gold bullet Conector. Разъёмы данного типа отличаются друг от друга диаметром и размером. Наиболее распространены разъёмы с диаметром коннектора 2 мм, 3 мм и 4 мм. 
 Часто используется для создания беспаечных соединений на PDB и моторах.
-
+![Banana](assets/banana.jpg)
-<img src="../assets/Banana.jpg" alt="Banana" width=200>

docs/copterhack2017.md

@@ -7,8 +7,6 @@ Copter Hack 2017
 
 <iframe width="560" height="315" src="https://www.youtube.com/embed/xgXheg3TTs4?rel=0" frameborder="0" allowfullscreen></iframe>
 
-Запись видеолекций – https://copterexpress.timepad.ru/event/510375/.
-
 Модули
 ---
 
@@ -36,7 +34,7 @@ SSID Wi-Fi
 
 Чтобы изменить SSID раздаваемого Wi-Fi необходимо любым способом изменить параметр ssid в файле ``/etc/hostapd/hostapd.conf``.
 
-Список распознанных маркеров
+Список разпознанных маркеров
 ---
 
 ```bash
@@ -52,14 +50,18 @@ rostopic echo /marker_data
 
 * [Пакет MAVRos](mavros.md)
 
-* Неплохая вводная статья: https://habrahabr.ru/post/227425/
+* Неплохая вводная статья
+https://habrahabr.ru/post/227425/ 
 
-* Сигналы, применяющиеся в дронах: https://geektimes.ru/post/258186/
+* Сигналы, применяющиеся в дронах
+https://geektimes.ru/post/258186/ 
 
-* Хорошая статья про ПИДы: https://habrahabr.ru/company/technoworks/blog/216437/
+* Хорошая статья про ПИДы
+https://habrahabr.ru/company/technoworks/blog/216437/
 
-* Запись видеолекций: https://copterexpress.timepad.ru/event/510375/
+* Запись видеолекций
+https://copterexpress.timepad.ru/event/510375/
 
 * [Aubio](https://aubio.org), библиотека для анализа звука (музыки)
 
-* Пакеты для работы с музыкой для Python: https://wiki.python.org/moin/PythonInMusic
+* Пакеты для работы с музыкой для Python https://wiki.python.org/moin/PythonInMusic

docs/deck.md

@@ -2,4 +2,4 @@
 Они абсолютно одинаковые. 
 Поэтому для дальнейшего удобства понимания инструкции условно разделим их на верхнюю и нижнюю дополнительные рамы
 
-![Общая раскладка](../assets/allElements.png)
+![Общая раскладка](assets/allElements.png)

docs/drugoe.md

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

docs/en/README.md

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

docs/en/SUMMARY.md

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

docs/en/book.json

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