aruco_map: start to work on mapping

2026-06-03 08:29:32 +00:00 · 2019-06-26 20:34:04 +03:00
35 changed files with 404 additions and 543 deletions
--- a/README.md
+++ b/README.md
@@ -10,7 +10,7 @@ Copter Express has implemented a large number of different autonomous drone proj

 Use it to learn how to assemble, configure, pilot and program autonomous CLEVER drone.

-## Raspberry Pi image
+## Preconfigured RPi 3 image

 **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).**

@@ -30,59 +30,49 @@ API description (in Russian) for autonomous flights is available [on GitBook](ht

 ## Manual installation

-Install ROS Kinetic according to the [documentation](http://wiki.ros.org/kinetic/Installation), then [create a Catkin workspace](http://wiki.ros.org/catkin/Tutorials/create_a_workspace).
+Install ROS Kinetic according to the [documentation](http://wiki.ros.org/kinetic/Installation).

-Clone this repo to directory `~/catkin_ws/src/clever`:
+Clone repo to directory `/home/pi/catkin_ws/src/clever`:

 ```bash
 cd ~/catkin_ws/src
 git clone https://github.com/CopterExpress/clever.git clever
 ```

-All the required ROS packages (including `mavros` and `opencv`) can be installed using `rosdep`:
-
-```bash
-cd ~/catkin_ws/
-rosdep install -y --from-paths src --ignore-src
-```
-
-Build ROS packages (on memory constrained platforms you might be going to need to use `-j1` key):
+Build ROS packages:

 ```bash
 cd ~/catkin_ws
 catkin_make -j1
 ```

-To complete `mavros` install you'll need to install `geographiclib` datasets:
+Enable systemd service `roscore` (if not enabled):

 ```bash
-curl https://raw.githubusercontent.com/mavlink/mavros/master/mavros/scripts/install_geographiclib_datasets.sh | sudo bash
-```
-
-## Running
-
-Enable systemd service `roscore` (if not running):
-
-```bash
-sudo systemctl enable /home/<username>/catkin_ws/src/clever/deploy/roscore.service
+sudo systemctl enable /home/pi/catkin_ws/src/clever/deploy/roscore.service
 sudo systemctl start roscore
 ```

-To start connection to SITL, use:
+Enable systemd service `clever`:

 ```bash
-roslaunch clever sitl.launch
-```
-
-To start connection to the flight controller, use:
-
-```bash
-roslaunch clever clever.launch
-```
-
-Also, you can enable and start the systemd service:
-
-```bash
-sudo systemctl enable /home/<username>/catkin_ws/src/clever/deploy/clever.service
+sudo systemctl enable /home/pi/catkin_ws/src/clever/deploy/clever.service
 sudo systemctl start clever
 ```
+
+### Dependencies
+
+[ROS Kinetic](http://wiki.ros.org/kinetic).
+
+Necessary ROS packages:
+
+* `opencv3`
+* `mavros`
+* `rosbridge_suite`
+* `web_video_server`
+* `cv_camera`
+* `nodelet`
+* `dynamic_reconfigure`
+* `bondcpp`, branch `master`
+* `roslint`
+* `rosserial`
--- a/aruco_pose/CMakeLists.txt
+++ b/aruco_pose/CMakeLists.txt
@@ -219,5 +219,4 @@ if (CATKIN_ENABLE_TESTING)
  add_rostest(test/test_parser_pass.test)
  add_rostest(test/test_parser_empty_map.test)
  add_rostest(test/test_node_failure.test)
-  add_rostest(test/largemap.test)
 endif()
--- a/aruco_pose/README.md
+++ b/aruco_pose/README.md
@@ -110,7 +110,7 @@ See examples in [`map`](map/) directory.
 Command for running tests:

 ```bash
-catkin_make run_tests && catkin_test_results
+rostest aruco_pose basic.test
 ```

 ## Copyright
--- a/aruco_pose/package.xml
+++ b/aruco_pose/package.xml
@@ -30,7 +30,6 @@
  <depend>rostest</depend>

  <test_depend>image_publisher</test_depend>
-  <test_depend>ros_pytest</test_depend>

  <!-- The export tag contains other, unspecified, tags -->
  <export>
--- a/aruco_pose/src/aruco_map.cpp
+++ b/aruco_pose/src/aruco_map.cpp
@@ -18,7 +18,6 @@
 #include <string>
 #include <vector>
 #include <fstream>
-#include <algorithm>
 #include <ros/ros.h>
 #include <nodelet/nodelet.h>
 #include <pluginlib/class_list_macros.h>
@@ -28,7 +27,6 @@
 #include <tf2_ros/buffer.h>
 #include <tf2_ros/transform_listener.h>
 #include <tf2_ros/transform_broadcaster.h>
-#include <tf2_ros/static_transform_broadcaster.h>
 #include <tf2_geometry_msgs/tf2_geometry_msgs.h>
 #include <message_filters/subscriber.h>
 #include <message_filters/synchronizer.h>
@@ -38,6 +36,7 @@
 #include <sensor_msgs/Image.h>
 #include <visualization_msgs/Marker.h>
 #include <visualization_msgs/MarkerArray.h>
+#include <algorithm>

 #include <aruco_pose/MarkerArray.h>
 #include <aruco_pose/Marker.h>
@@ -69,15 +68,16 @@ private:
 	Mat camera_matrix_, dist_coeffs_;
 	geometry_msgs::TransformStamped transform_;
 	geometry_msgs::PoseWithCovarianceStamped pose_;
-	vector<geometry_msgs::TransformStamped> markers_transforms_;
 	tf2_ros::TransformBroadcaster br_;
-	tf2_ros::StaticTransformBroadcaster static_br_;
 	tf2_ros::Buffer tf_buffer_;
 	tf2_ros::TransformListener tf_listener_{tf_buffer_};
 	visualization_msgs::MarkerArray vis_array_;
-	std::string known_tilt_, map_, markers_frame_, markers_parent_frame_;
+	std::string known_tilt_;
 	int image_width_, image_height_, image_margin_;
 	bool auto_flip_;
+	bool mapping_;
+	vector<int> mapping_exclude_;
+	std::fstream map_file_;

 public:
 	virtual void onInit()
@@ -101,11 +101,11 @@ public:
 		nh_priv_.param<std::string>("frame_id", transform_.child_frame_id, "aruco_map");
 		nh_priv_.param<std::string>("known_tilt", known_tilt_, "");
 		nh_priv_.param("auto_flip", auto_flip_, false);
+		nh_priv_.param("mapping", mapping_, false);
+		nh_priv_.getParam("mapping_exclude", mapping_exclude_);
 		nh_priv_.param("image_width", image_width_, 2000);
 		nh_priv_.param("image_height", image_height_, 2000);
 		nh_priv_.param("image_margin", image_margin_, 200);
-		nh_priv_.param<std::string>("markers/frame_id", markers_parent_frame_, transform_.child_frame_id);
-		nh_priv_.param<std::string>("markers/child_frame_id_prefix", markers_frame_, "");

 		// createStripLine();

@@ -130,7 +130,6 @@ public:
 		sync_.reset(new message_filters::Synchronizer<SyncPolicy>(SyncPolicy(10), image_sub_, info_sub_, markers_sub_));
 		sync_->registerCallback(boost::bind(&ArucoMap::callback, this, _1, _2, _3));

-		publishMarkersFrames();
 		publishMapImage();
 		vis_markers_pub_.publish(vis_array_);

@@ -162,6 +161,13 @@ public:
 				cv::Point2f(marker.c4.x, marker.c4.y)
 			};
 			corners.push_back(marker_corners);
+
+			if (mapping_) {
+				if(std::find(board_->ids.begin(), board_->ids.end(), marker.id) == board_->ids.end()) {
+					// no such marker in map
+					mappingAddMarker(marker);
+				}
+			}
 		}

 		if (known_tilt_.empty()) {
@@ -263,15 +269,15 @@ publish_debug:

 	void loadMap(std::string filename)
 	{
-		std::ifstream f(filename);
+		map_file_.open(filename, std::fstream::in | std::fstream::out | std::fstream::app);
 		std::string line;

-		if (!f.good()) {
+		if (!map_file_.good()) {
 			ROS_FATAL("aruco_map: %s - %s", strerror(errno), filename.c_str());
 			ros::shutdown();
 		}

-		while (std::getline(f, line)) {
+		while (std::getline(map_file_, line)) {
 			int id;
 			double length, x, y, z, yaw, pitch, roll;

@@ -324,9 +330,38 @@ publish_debug:
 			addMarker(id, length, x, y, z, yaw, pitch, roll);
 		}

+		map_file_.clear(); // clear EOF state
 		ROS_INFO("aruco_map: loading %s complete (%d markers)", filename.c_str(), static_cast<int>(board_->ids.size()));
 	}

+	void mappingAddMarker(const aruco_pose::Marker& marker)
+	{
+		ROS_INFO("aruco_map: add marker %d to map", marker.id);
+
+		double roll, pitch, yaw;
+		tf::Quaternion q;
+		tf::quaternionMsgToTF(marker.pose.orientation, q);
+		tf::Matrix3x3(q).getRPY(roll, pitch, yaw);
+		const geometry_msgs::Point& p = marker.pose.position;
+		addMarker(marker.id, marker.length, p.x, p.y, p.z, yaw, pitch, roll);
+		writeToMap(marker.id, marker.length, p.x, p.y, p.z, yaw, pitch, roll);
+		publishMapImage();
+		vis_markers_pub_.publish(vis_array_);
+	}
+
+	inline void writeToMap(int id, double length, double x, double y, double z, double yaw, double pitch, double roll)
+	{
+		map_file_ << std::setprecision(3) // round numbers
+		          << id << '\t'
+		          << length << '\t'
+		          << x << '\t'
+		          << y << '\t'
+		          << z << '\t'
+		          << yaw << '\t'
+		          << pitch << '\t'
+		          << roll << std::endl;
+	}
+
 	void createGridBoard()
 	{
 		ROS_INFO("aruco_map: generate gridboard");
@@ -428,15 +463,6 @@ publish_debug:
 		board_->ids.push_back(id);
 		board_->objPoints.push_back(obj_points);

-		// Add marker's static transform
-		if (!markers_frame_.empty()) {
-			geometry_msgs::TransformStamped marker_transform;
-			marker_transform.header.frame_id = markers_parent_frame_;
-			marker_transform.child_frame_id = markers_frame_ + std::to_string(id);
-			tf::transformTFToMsg(transform, marker_transform.transform);
-			markers_transforms_.push_back(marker_transform);
-		}
-
 		// Add visualization marker
 		visualization_msgs::Marker marker;
 		marker.header.frame_id = transform_.child_frame_id;
@@ -467,13 +493,6 @@ publish_debug:
 		// vis_array_.markers.at(0).points.push_back(p);
 	}

-	void publishMarkersFrames()
-	{
-		if (!markers_transforms_.empty()) {
-			static_br_.sendTransform(markers_transforms_);
-		}
-	}
-
 	void publishMapImage()
 	{
 		cv::Size size(image_width_, image_height_);
--- a/aruco_pose/src/draw.cpp
+++ b/aruco_pose/src/draw.cpp
@@ -87,7 +87,6 @@ void _drawPlanarBoard(Board *_board, Size outSize, OutputArray _img, int marginS
 		// dst_sz.width = dst_sz.height = std::min(dst_sz.width, dst_sz.height); //marker should be square
 		double diag = std::round(std::hypot(dst_sz.width, dst_sz.height));
 		int side = std::round(diag / std::sqrt(2));
-		side = std::max(side, 10);

 		dictionary.drawMarker(_board->ids[m], side, marker, borderBits);

--- a/aruco_pose/test/basic.py
+++ b/aruco_pose/test/basic.py
@@ -1,131 +1,101 @@
+#!/usr/bin/env python
+import sys
+import unittest
+import json
 import rospy
-import pytest
+import rostest

-import tf2_ros
-import tf2_geometry_msgs
 from geometry_msgs.msg import PoseWithCovarianceStamped
 from sensor_msgs.msg import Image
 from aruco_pose.msg import MarkerArray
 from visualization_msgs.msg import MarkerArray as VisMarkerArray


-@pytest.fixture
-def node():
-    return rospy.init_node('aruco_pose_test', anonymous=True)
+class TestArucoPose(unittest.TestCase):
+    def setUp(self):
+        rospy.init_node('test_aruco_detect', anonymous=True)

-@pytest.fixture
-def tf_buffer():
-    buf = tf2_ros.Buffer()
-    tf2_ros.TransformListener(buf)
-    return buf
+    def test_markers(self):
+        markers = rospy.wait_for_message('aruco_detect/markers', MarkerArray, timeout=5)
+        self.assertEqual(len(markers.markers), 4)
+        self.assertEqual(markers.header.frame_id, 'main_camera_optical')

-def approx(expected):
-    return pytest.approx(expected, abs=1e-4) # compare floats more roughly
+        self.assertEqual(markers.markers[0].id, 2)
+        self.assertAlmostEqual(markers.markers[0].length, 0.33, places=4)
+        self.assertAlmostEqual(markers.markers[0].pose.position.x, 0.36706567854, places=4)
+        self.assertAlmostEqual(markers.markers[0].pose.position.y, 0.290484516644, places=4)
+        self.assertAlmostEqual(markers.markers[0].pose.position.z, 2.18787602301, places=4)
+        self.assertAlmostEqual(markers.markers[0].pose.orientation.x, 0.993997406299, places=4)
+        self.assertAlmostEqual(markers.markers[0].pose.orientation.y, -0.00532003481626, places=4)
+        self.assertAlmostEqual(markers.markers[0].pose.orientation.z, -0.107390951553, places=4)
+        self.assertAlmostEqual(markers.markers[0].pose.orientation.w, 0.0201999263402, places=4)
+        self.assertAlmostEqual(markers.markers[0].c1.x, 415.557739258, places=4)
+        self.assertAlmostEqual(markers.markers[0].c1.y, 335.557739258, places=4)
+        self.assertAlmostEqual(markers.markers[0].c2.x, 509.442260742, places=4)
+        self.assertAlmostEqual(markers.markers[0].c2.y, 335.557739258, places=4)
+        self.assertAlmostEqual(markers.markers[0].c3.x, 509.442260742, places=4)
+        self.assertAlmostEqual(markers.markers[0].c3.y, 429.442260742, places=4)
+        self.assertAlmostEqual(markers.markers[0].c4.x, 415.557739258, places=4)
+        self.assertAlmostEqual(markers.markers[0].c4.y, 429.442260742, places=4)

-def test_markers(node):
-    markers = rospy.wait_for_message('aruco_detect/markers', MarkerArray, timeout=5)
-    assert len(markers.markers) == 4
-    assert markers.header.frame_id == 'main_camera_optical'
+        self.assertEqual(markers.markers[3].id, 3)
+        self.assertAlmostEqual(markers.markers[3].length, 0.1, places=4)
+        self.assertAlmostEqual(markers.markers[3].pose.position.x, -0.1805169666, places=4)
+        self.assertAlmostEqual(markers.markers[3].pose.position.y, -0.200697302327, places=4)
+        self.assertAlmostEqual(markers.markers[3].pose.position.z, 0.585767514823, places=4)
+        self.assertAlmostEqual(markers.markers[3].pose.orientation.x, -0.961738074009, places=4)
+        self.assertAlmostEqual(markers.markers[3].pose.orientation.y, -0.0375180244707, places=4)
+        self.assertAlmostEqual(markers.markers[3].pose.orientation.z, -0.0115387773672, places=4)
+        self.assertAlmostEqual(markers.markers[3].pose.orientation.w, 0.271144115664, places=4)
+        self.assertAlmostEqual(markers.markers[3].c1.x, 129.557723999, places=4)
+        self.assertAlmostEqual(markers.markers[3].c1.y, 49.557723999, places=4)
+        self.assertAlmostEqual(markers.markers[3].c2.x, 223.442276001, places=4)
+        self.assertAlmostEqual(markers.markers[3].c2.y, 49.557723999, places=4)
+        self.assertAlmostEqual(markers.markers[3].c3.x, 223.442276001, places=4)
+        self.assertAlmostEqual(markers.markers[3].c3.y, 143.442276001, places=4)
+        self.assertAlmostEqual(markers.markers[3].c4.x, 129.557723999, places=4)
+        self.assertAlmostEqual(markers.markers[3].c4.y, 143.442276001, places=4)

-    assert markers.markers[0].id == 2
-    assert markers.markers[0].length == approx(0.33)
-    assert markers.markers[0].pose.position.x == approx(0.36706567854)
-    assert markers.markers[0].pose.position.y == approx(0.290484516644)
-    assert markers.markers[0].pose.position.z == approx(2.18787602301)
-    assert markers.markers[0].pose.orientation.x == approx(0.993997406299)
-    assert markers.markers[0].pose.orientation.y == approx(-0.00532003481626)
-    assert markers.markers[0].pose.orientation.z == approx(-0.107390951553)
-    assert markers.markers[0].pose.orientation.w == approx(0.0201999263402)
-    assert markers.markers[0].c1.x == approx(415.557739258)
-    assert markers.markers[0].c1.y == approx(335.557739258)
-    assert markers.markers[0].c2.x == approx(509.442260742)
-    assert markers.markers[0].c2.y == approx(335.557739258)
-    assert markers.markers[0].c3.x == approx(509.442260742)
-    assert markers.markers[0].c3.y == approx(429.442260742)
-    assert markers.markers[0].c4.x == approx(415.557739258)
-    assert markers.markers[0].c4.y == approx(429.442260742)
+        self.assertEqual(markers.markers[1].id, 1)
+        self.assertAlmostEqual(markers.markers[1].length, 0.33, places=4)
+        self.assertEqual(markers.markers[2].id, 4)
+        self.assertAlmostEqual(markers.markers[2].length, 0.33, places=4)

-    assert markers.markers[3].id == 3
-    assert markers.markers[3].length == approx(0.1)
-    assert markers.markers[3].pose.position.x == approx(-0.1805169666)
-    assert markers.markers[3].pose.position.y == approx(-0.200697302327)
-    assert markers.markers[3].pose.position.z == approx(0.585767514823)
-    assert markers.markers[3].pose.orientation.x == approx(-0.961738074009)
-    assert markers.markers[3].pose.orientation.y == approx(-0.0375180244707)
-    assert markers.markers[3].pose.orientation.z == approx(-0.0115387773672)
-    assert markers.markers[3].pose.orientation.w == approx(0.271144115664)
-    assert markers.markers[3].c1.x == approx(129.557723999)
-    assert markers.markers[3].c1.y == approx(49.557723999)
-    assert markers.markers[3].c2.x == approx(223.442276001)
-    assert markers.markers[3].c2.y == approx(49.557723999)
-    assert markers.markers[3].c3.x == approx(223.442276001)
-    assert markers.markers[3].c3.y == approx(143.442276001)
-    assert markers.markers[3].c4.x == approx(129.557723999)
-    assert markers.markers[3].c4.y == approx(143.442276001)
+    def test_visualization(self):
+        vis = rospy.wait_for_message('aruco_detect/visualization', VisMarkerArray, timeout=5)
+        self.assertEqual(len(vis.markers), 9)

-    assert markers.markers[1].id == 1
-    assert markers.markers[1].length == approx(0.33)
-    assert markers.markers[2].id == 4
-    assert markers.markers[2].length == approx(0.33)
+    def test_debug(self):
+        img = rospy.wait_for_message('aruco_detect/debug', Image, timeout=5)
+        self.assertEqual(img.width, 640)
+        self.assertEqual(img.height, 480)
+        self.assertEqual(img.header.frame_id, 'main_camera_optical')

-def test_markers_frames(node, tf_buffer):
-    marker_2 = tf_buffer.lookup_transform('main_camera_optical', 'aruco_2', rospy.Time(), rospy.Duration(5))
-    assert marker_2.transform.translation.x == approx(0.36706567854)
-    assert marker_2.transform.translation.y == approx(0.290484516644)
-    assert marker_2.transform.translation.z == approx(2.18787602301)
-    assert marker_2.transform.rotation.x == approx(0.993997406299)
-    assert marker_2.transform.rotation.y == approx(-0.00532003481626)
-    assert marker_2.transform.rotation.z == approx(-0.107390951553)
-    assert marker_2.transform.rotation.w == approx(0.0201999263402)
+    def test_map(self):
+        pose = rospy.wait_for_message('aruco_map/pose', PoseWithCovarianceStamped, timeout=5)
+        self.assertEqual(pose.header.frame_id, 'main_camera_optical')
+        self.assertAlmostEqual(pose.pose.pose.position.x, -0.629167753342, places=4)
+        self.assertAlmostEqual(pose.pose.pose.position.y, 0.293822650809, places=4)
+        self.assertAlmostEqual(pose.pose.pose.position.z, 2.12641343155, places=4)
+        self.assertAlmostEqual(pose.pose.pose.orientation.x, -0.998383794799, places=4)
+        self.assertAlmostEqual(pose.pose.pose.orientation.y, -5.20919098575e-06, places=4)
+        self.assertAlmostEqual(pose.pose.pose.orientation.z, -0.0300861070302, places=4)
+        self.assertAlmostEqual(pose.pose.pose.orientation.w, 0.0482143590507, places=4)

-def test_map_markers_frames(node, tf_buffer):
-    stamp = rospy.get_rostime()
-    timeout = rospy.Duration(5)
+    def test_map_image(self):
+        img = rospy.wait_for_message('aruco_map/image', Image, timeout=5)
+        self.assertEqual(img.width, 2000)
+        self.assertEqual(img.height, 2000)
+        self.assertEqual(img.encoding, 'mono8')

-    marker_1 = tf_buffer.lookup_transform('aruco_map', 'aruco_map_1', stamp, timeout)
-    assert marker_1.transform.translation.x == approx(0)
-    assert marker_1.transform.translation.y == approx(0)
-    assert marker_1.transform.translation.z == approx(0)
+    def test_map_visualization(self):
+        vis = rospy.wait_for_message('aruco_map/visualization', VisMarkerArray, timeout=5)

-    marker_4 = tf_buffer.lookup_transform('aruco_map', 'aruco_map_4', stamp, timeout)
-    assert marker_4.transform.translation.x == approx(1)
-    assert marker_4.transform.translation.y == approx(1)
-    assert marker_4.transform.translation.z == approx(0)
+    def test_map_debug(self):
+        img = rospy.wait_for_message('aruco_map/debug', Image, timeout=5)

-    marker_12 = tf_buffer.lookup_transform('aruco_map', 'aruco_map_12', stamp, timeout)
-    assert marker_12.transform.translation.x == approx(0.2)
-    assert marker_12.transform.translation.y == approx(0.5)
-    assert marker_12.transform.translation.z == approx(0)
+    # def test_transforms(self):
+    #     pass

-def test_visualization(node):
-    vis = rospy.wait_for_message('aruco_detect/visualization', VisMarkerArray, timeout=5)
-    assert len(vis.markers) == 9

-def test_debug(node):
-    img = rospy.wait_for_message('aruco_detect/debug', Image, timeout=5)
-    assert img.width == 640
-    assert img.height == 480
-    assert img.header.frame_id == 'main_camera_optical'
-
-def test_map(node):
-    pose = rospy.wait_for_message('aruco_map/pose', PoseWithCovarianceStamped, timeout=5)
-    assert pose.header.frame_id == 'main_camera_optical'
-    assert pose.pose.pose.position.x == approx(-0.629167753342)
-    assert pose.pose.pose.position.y == approx(0.293822650809)
-    assert pose.pose.pose.position.z == approx(2.12641343155)
-    assert pose.pose.pose.orientation.x == approx(-0.998383794799)
-    assert pose.pose.pose.orientation.y == approx(-5.20919098575e-06)
-    assert pose.pose.pose.orientation.z == approx(-0.0300861070302)
-    assert pose.pose.pose.orientation.w == approx(0.0482143590507)
-
-def test_map_image(node):
-    img = rospy.wait_for_message('aruco_map/image', Image, timeout=5)
-    assert img.width == 2000
-    assert img.height == 2000
-    assert img.encoding == 'mono8'
-
-def test_map_visualization(node):
-    vis = rospy.wait_for_message('aruco_map/visualization', VisMarkerArray, timeout=5)
-
-def test_map_debug(node):
-    img = rospy.wait_for_message('aruco_map/debug', Image, timeout=5)
+rostest.rosrun('aruco_pose', 'test_aruco_detect', TestArucoPose, sys.argv)
--- a/aruco_pose/test/basic.test
+++ b/aruco_pose/test/basic.test
@@ -5,27 +5,23 @@
        <param name="camera_info_url" value="file://$(find aruco_pose)/test/camera_info.yaml" />
    </node>

-    <node pkg="nodelet" type="nodelet" name="nodelet_manager" args="manager" required="true"/>
+    <node pkg="nodelet" type="nodelet" name="nodelet_manager" args="manager"/>

-    <node pkg="nodelet" clear_params="true" type="nodelet" name="aruco_detect" args="load aruco_pose/aruco_detect nodelet_manager" required="true">
+    <node pkg="nodelet" clear_params="true" type="nodelet" name="aruco_detect" args="load aruco_pose/aruco_detect nodelet_manager">
        <remap from="image_raw" to="main_camera/image_raw"/>
        <remap from="camera_info" to="main_camera/camera_info"/>
        <param name="length" value="0.33"/>
        <param name="length_override/3" value="0.1"/>
        <param name="estimate_poses" value="true"/>
-        <param name="send_tf" value="true"/>
    </node>

-    <node name="aruco_map" pkg="nodelet" type="nodelet" args="load aruco_pose/aruco_map nodelet_manager" clear_params="true" required="true">
+    <node name="aruco_map" pkg="nodelet" type="nodelet" args="load aruco_pose/aruco_map nodelet_manager" clear_params="true">
        <remap from="image_raw" to="main_camera/image_raw"/>
        <remap from="camera_info" to="main_camera/camera_info"/>
        <remap from="markers" to="aruco_detect/markers"/>
        <param name="type" value="map"/>
        <param name="map" value="$(find aruco_pose)/test/basic.txt"/>
-        <param name="markers/frame_id" value="aruco_map"/>
-        <param name="markers/child_frame_id_prefix" value="aruco_map_"/>
    </node>

-    <param name="test_module" value="$(find aruco_pose)/test/basic.py"/>
-    <test test-name="aruco_pose_test" pkg="ros_pytest" type="ros_pytest_runner"/>
+    <test test-name="test_aruco_pose" pkg="aruco_pose" type="basic.py"/>
 </launch>
--- a/aruco_pose/test/largemap.py
+++ b/aruco_pose/test/largemap.py
@@ -1,26 +0,0 @@
-#!/usr/bin/env python
-import sys
-import unittest
-import json
-import rospy
-import rostest
-
-from sensor_msgs.msg import Image
-from visualization_msgs.msg import MarkerArray as VisMarkerArray
-
-
-class TestArucoPose(unittest.TestCase):
-    def setUp(self):
-        rospy.init_node('test_aruco_largemap', anonymous=True)
-
-    def test_map_image(self):
-        img = rospy.wait_for_message('aruco_map/image', Image, timeout=5)
-        self.assertEqual(img.width, 2000)
-        self.assertEqual(img.height, 2000)
-        self.assertEqual(img.encoding, 'mono8')
-
-    def test_map_visualization(self):
-        vis = rospy.wait_for_message('aruco_map/visualization', VisMarkerArray, timeout=5)
-
-
-rostest.rosrun('aruco_pose', 'test_aruco_largemap', TestArucoPose, sys.argv)
--- a/aruco_pose/test/largemap.test
+++ b/aruco_pose/test/largemap.test
@@ -1,13 +0,0 @@
-<launch>
-    <node pkg="nodelet" type="nodelet" name="nodelet_manager" args="manager" required="true"/>
-
-    <node name="aruco_map" pkg="nodelet" type="nodelet" args="load aruco_pose/aruco_map nodelet_manager" clear_params="true" required="true">
-        <remap from="image_raw" to="main_camera/image_raw"/>
-        <remap from="camera_info" to="main_camera/camera_info"/>
-        <remap from="markers" to="aruco_detect/markers"/>
-        <param name="type" value="map"/>
-        <param name="map" value="$(find aruco_pose)/test/largemap.txt"/>
-    </node>
-
-    <test test-name="test_aruco_pose_largemap" pkg="aruco_pose" type="largemap.py"/>
-</launch>
--- a/aruco_pose/test/largemap.txt
+++ b/aruco_pose/test/largemap.txt
@@ -1,11 +0,0 @@
-0 0.2 0 0 0 0 0 0
-1 0.2 10 0 0 0 0 0
-2 0.2 20 0 0 0 0 0
-3 0.2 30 0 0 0 0 0
-4 0.2 40 0 0 0 0 0
-5 0.2 50 0 0 0 0 0
-6 0.2 60 0 0 0 0 0
-7 0.2 70 0 0 0 0 0
-8 0.2 80 0 0 0 0 0
-9 0.2 90 0 0 0 0 0
-10 0.2 100 0 0 0 0 0
--- a/aruco_pose/test/test_node_failure.py
+++ b/aruco_pose/test/test_node_failure.py
@@ -1,13 +1,27 @@
+#!/usr/bin/env python
+import sys
+import unittest
+import json
 import rospy
-import pytest
+import rostest

+from geometry_msgs.msg import PoseWithCovarianceStamped
+from sensor_msgs.msg import Image
+from aruco_pose.msg import MarkerArray
 from visualization_msgs.msg import MarkerArray as VisMarkerArray


-@pytest.fixture
-def node():
-    return rospy.init_node('aruco_pose_test', anonymous=True)
+class TestArucoMapPass(unittest.TestCase):
+    def setUp(self):
+        rospy.init_node('test_parser_fail', anonymous=True)

-def test_node_failure(node):
-    with pytest.raises(rospy.exceptions.ROSException):
-        rospy.wait_for_message('aruco_map/visualization', VisMarkerArray, timeout=5)
+    def test_node_failure(self):
+        try:
+            markers = rospy.wait_for_message('aruco_map/visualization', VisMarkerArray, timeout=5)
+            did_post_message = True
+        except rospy.exceptions.ROSException:
+            did_post_message = False
+        self.assertFalse(did_post_message)
+
+
+rostest.rosrun('aruco_pose', 'test_aruco_map', TestArucoMapPass, sys.argv)
--- a/aruco_pose/test/test_node_failure.test
+++ b/aruco_pose/test/test_node_failure.test
@@ -9,6 +9,5 @@
        <param name="map" value="$(find aruco_pose)/test/test_node_failure.txt"/>
    </node>

-    <param name="test_module" value="$(find aruco_pose)/test/test_node_failure.py"/>
-    <test test-name="test_node_failure" pkg="ros_pytest" type="ros_pytest_runner"/>
+    <test test-name="test_aruco_map_fail_dict" pkg="aruco_pose" type="test_node_failure.py"/>
 </launch>
--- a/aruco_pose/test/test_parser_empty_map.py
+++ b/aruco_pose/test/test_parser_empty_map.py
@@ -1,13 +1,24 @@
+#!/usr/bin/env python
+import sys
+import unittest
+import json
 import rospy
-import pytest
+import rostest

+from geometry_msgs.msg import PoseWithCovarianceStamped
+from sensor_msgs.msg import Image
+from aruco_pose.msg import MarkerArray
 from visualization_msgs.msg import MarkerArray as VisMarkerArray


-@pytest.fixture
-def node():
-    return rospy.init_node('aruco_pose_test_empty_map', anonymous=True)
+class TestArucoMapPass(unittest.TestCase):
+    def setUp(self):
+        rospy.init_node('test_parser_fail', anonymous=True)

-def test_empty_map(node):
-    markers = rospy.wait_for_message('aruco_map/visualization', VisMarkerArray, timeout=5)
-    assert len(markers.markers) == 0
+
+    def test_node_failure(self):
+        markers = rospy.wait_for_message('aruco_map/visualization', VisMarkerArray, timeout=5)
+        self.assertEquals(len(markers.markers), 0)
+
+
+rostest.rosrun('aruco_pose', 'test_aruco_map', TestArucoMapPass, sys.argv)
--- a/aruco_pose/test/test_parser_empty_map.test
+++ b/aruco_pose/test/test_parser_empty_map.test
@@ -9,6 +9,5 @@
        <param name="map" value="$(find aruco_pose)/test/test_parser_empty_map.txt"/>
    </node>

-    <param name="test_module" value="$(find aruco_pose)/test/test_parser_empty_map.py"/>
-    <test test-name="test_node_empty_map" pkg="ros_pytest" type="ros_pytest_runner"/>
+    <test test-name="test_aruco_map_incomplete" pkg="aruco_pose" type="test_parser_empty_map.py"/>
 </launch>
--- a/aruco_pose/test/test_parser_pass.py
+++ b/aruco_pose/test/test_parser_pass.py
@@ -1,61 +1,75 @@
+#!/usr/bin/env python
+import sys
+import unittest
+import json
 import rospy
-import pytest
+import rostest

+from geometry_msgs.msg import PoseWithCovarianceStamped
 from sensor_msgs.msg import Image
 from aruco_pose.msg import MarkerArray
 from visualization_msgs.msg import MarkerArray as VisMarkerArray


-@pytest.fixture
-def node():
-    return rospy.init_node('aruco_pose_test', anonymous=True)
+class TestArucoMapPass(unittest.TestCase):
+    def setUp(self):
+        rospy.init_node('test_parser_pass', anonymous=True)

-def approx(expected):
-    return pytest.approx(expected, abs=1e-4) # compare floats more roughly
+    def test_markers(self):
+        markers = rospy.wait_for_message('aruco_map/visualization', VisMarkerArray, timeout=5)
+        
+        self.assertEqual(len(markers.markers), 6)
+# FIXME: visual marker id is not ArUco marker id
+#        self.assertEqual(markers.markers[0].id, 1)
+#        self.assertEqual(markers.markers[1].id, 2)
+#        self.assertEqual(markers.markers[2].id, 3)
+#        self.assertEqual(markers.markers[3].id, 4)

-def test_markers(node):
-    markers = rospy.wait_for_message('aruco_map/visualization', VisMarkerArray, timeout=5)
-    assert len(markers.markers) == 6
+        self.assertAlmostEqual(markers.markers[0].pose.position.x, 0, places=7)
+        self.assertAlmostEqual(markers.markers[0].pose.position.y, 0, places=7)
+        self.assertAlmostEqual(markers.markers[0].pose.position.z, 0, places=7)
+        
+        self.assertAlmostEqual(markers.markers[1].pose.position.x, 1, places=7)
+        self.assertAlmostEqual(markers.markers[1].pose.position.y, 1, places=7)
+        self.assertAlmostEqual(markers.markers[1].pose.position.z, 1, places=7)
+        
+        self.assertAlmostEqual(markers.markers[2].pose.position.x, 1, places=7)
+        self.assertAlmostEqual(markers.markers[2].pose.position.y, 0, places=7)
+        self.assertAlmostEqual(markers.markers[2].pose.position.z, 0.5, places=7)
+        
+        self.assertAlmostEqual(markers.markers[3].pose.position.x, 0, places=7)
+        self.assertAlmostEqual(markers.markers[3].pose.position.y, 1, places=7)
+        self.assertAlmostEqual(markers.markers[3].pose.position.z, 0, places=7)
+        
+        self.assertAlmostEqual(markers.markers[4].pose.position.x, 1, places=7)
+        self.assertAlmostEqual(markers.markers[4].pose.position.y, 0.5, places=7)
+        self.assertAlmostEqual(markers.markers[4].pose.position.z, 0, places=7)
+        
+        self.assertAlmostEqual(markers.markers[5].pose.position.x, 2.2, places=7)
+        self.assertAlmostEqual(markers.markers[5].pose.position.y, 0.2, places=7)
+        self.assertAlmostEqual(markers.markers[5].pose.position.z, 0, places=7)

-    assert markers.markers[0].pose.position.x == approx(0)
-    assert markers.markers[0].pose.position.y == approx(0)
-    assert markers.markers[0].pose.position.z == approx(0)
+        self.assertAlmostEqual(markers.markers[0].scale.x, 0.33, places=7)
+        self.assertAlmostEqual(markers.markers[0].scale.y, 0.33, places=7)
+        self.assertAlmostEqual(markers.markers[1].scale.x, 0.225, places=7)
+        self.assertAlmostEqual(markers.markers[1].scale.y, 0.225, places=7)
+        self.assertAlmostEqual(markers.markers[2].scale.x, 0.45, places=7)
+        self.assertAlmostEqual(markers.markers[2].scale.y, 0.45, places=7)
+        self.assertAlmostEqual(markers.markers[3].scale.x, 0.15, places=7)
+        self.assertAlmostEqual(markers.markers[3].scale.y, 0.15, places=7)
+        self.assertAlmostEqual(markers.markers[4].scale.x, 0.25, places=7)
+        self.assertAlmostEqual(markers.markers[4].scale.y, 0.25, places=7)
+        self.assertAlmostEqual(markers.markers[5].scale.x, 0.35, places=7)
+        self.assertAlmostEqual(markers.markers[5].scale.y, 0.35, places=7)

-    assert markers.markers[1].pose.position.x == approx(1)
-    assert markers.markers[1].pose.position.y == approx(1)
-    assert markers.markers[1].pose.position.z == approx(1)
+    def test_map_image(self):
+        img = rospy.wait_for_message('aruco_map/image', Image, timeout=5)
+        self.assertEqual(img.width, 2000)
+        self.assertEqual(img.height, 2000)
+        self.assertEqual(img.encoding, 'mono8')

-    assert markers.markers[2].pose.position.x == approx(1)
-    assert markers.markers[2].pose.position.y == approx(0)
-    assert markers.markers[2].pose.position.z == approx(0.5)
+    # def test_transforms(self):
+    #     pass

-    assert markers.markers[3].pose.position.x == approx(0)
-    assert markers.markers[3].pose.position.y == approx(1)
-    assert markers.markers[3].pose.position.z == approx(0)

-    assert markers.markers[4].pose.position.x == approx(1)
-    assert markers.markers[4].pose.position.y == approx(0.5)
-    assert markers.markers[4].pose.position.z == approx(0)
-
-    assert markers.markers[5].pose.position.x == approx(2.2)
-    assert markers.markers[5].pose.position.y == approx(0.2)
-    assert markers.markers[5].pose.position.z == approx(0)
-
-    assert markers.markers[0].scale.x == approx(0.33)
-    assert markers.markers[0].scale.y == approx(0.33)
-    assert markers.markers[1].scale.x == approx(0.225)
-    assert markers.markers[1].scale.y == approx(0.225)
-    assert markers.markers[2].scale.x == approx(0.45)
-    assert markers.markers[2].scale.y == approx(0.45)
-    assert markers.markers[3].scale.x == approx(0.15)
-    assert markers.markers[3].scale.y == approx(0.15)
-    assert markers.markers[4].scale.x == approx(0.25)
-    assert markers.markers[4].scale.y == approx(0.25)
-    assert markers.markers[5].scale.x == approx(0.35)
-    assert markers.markers[5].scale.y == approx(0.35)
-
-def test_map_image(node):
-    img = rospy.wait_for_message('aruco_map/image', Image, timeout=5)
-    assert img.width == 2000
-    assert img.height == 2000
-    assert img.encoding == 'mono8'
+rostest.rosrun('aruco_pose', 'test_aruco_map', TestArucoMapPass, sys.argv)
--- a/aruco_pose/test/test_parser_pass.test
+++ b/aruco_pose/test/test_parser_pass.test
@@ -1,7 +1,7 @@
 <launch>
-    <node pkg="nodelet" type="nodelet" name="nodelet_manager" args="manager" required="true"/>
+    <node pkg="nodelet" type="nodelet" name="nodelet_manager" args="manager"/>

-    <node name="aruco_map" pkg="nodelet" type="nodelet" args="load aruco_pose/aruco_map nodelet_manager" clear_params="true" required="true">
+    <node name="aruco_map" pkg="nodelet" type="nodelet" args="load aruco_pose/aruco_map nodelet_manager" clear_params="true">
        <remap from="image_raw" to="main_camera/image_raw"/>
        <remap from="camera_info" to="main_camera/camera_info"/>
        <remap from="markers" to="aruco_detect/markers"/>
@@ -9,6 +9,5 @@
        <param name="map" value="$(find aruco_pose)/test/test_parser_pass.txt"/>
    </node>

-    <param name="test_module" value="$(find aruco_pose)/test/test_parser_pass.py"/>
-    <test test-name="test_node_pass" pkg="ros_pytest" type="ros_pytest_runner"/>
+    <test test-name="test_aruco_map" pkg="aruco_pose" type="test_parser_pass.py"/>
 </launch>
--- a/builder/assets/kinetic-rosdep-clever.yaml
+++ b/builder/assets/kinetic-rosdep-clever.yaml
@@ -550,6 +550,3 @@ ddynamic_reconfigure:
 realsense2_camera:
  debian:
    stretch: [ros-kinetic-realsense2-camera]
-ros_pytest:
-  debian:
-    stretch: [ros-kinetic-ros-pytest]
--- a/builder/image-ros.sh
+++ b/builder/image-ros.sh
@@ -155,6 +155,8 @@ echo_stamp "Installing CLEVER" \
 && my_travis_retry pip install -r /home/pi/catkin_ws/src/clever/clever/requirements.txt \
 && source /opt/ros/kinetic/setup.bash \
 && catkin_make -j2 -DCMAKE_BUILD_TYPE=Release \
+&& catkin_make run_tests \
+&& catkin_test_results \
 && systemctl enable roscore \
 && systemctl enable clever \
 && echo_stamp "All CLEVER was installed!" "SUCCESS" \
@@ -169,6 +171,7 @@ gitbook build
 echo_stamp "Installing additional ROS packages"
 apt-get install -y --no-install-recommends \
    ros-kinetic-dynamic-reconfigure \
+    ros-kinetic-tf2-web-republisher \
    ros-kinetic-compressed-image-transport \
    ros-kinetic-rosbridge-suite \
    ros-kinetic-rosserial \
@@ -178,12 +181,9 @@ apt-get install -y --no-install-recommends \
    ros-kinetic-rosshow

 # TODO move GeographicLib datasets to Mavros debian package
-echo_stamp "Install GeographicLib datasets (needed for mavros)" \
+echo_stamp "Install GeographicLib datasets (needs for mavros)" \
 && wget -qO- https://raw.githubusercontent.com/mavlink/mavros/master/mavros/scripts/install_geographiclib_datasets.sh | bash

-echo_stamp "Running tests"
-catkin_make run_tests && catkin_test_results
-
 echo_stamp "Change permissions for catkin_ws"
 chown -Rf pi:pi /home/pi/catkin_ws

--- a/builder/image-software.sh
+++ b/builder/image-software.sh
@@ -108,7 +108,6 @@ ntpdate \
 python-dev \
 python3-dev \
 python-systemd \
-mjpg-streamer=2.0 \
 && echo_stamp "Everything was installed!" "SUCCESS" \
 || (echo_stamp "Some packages wasn't installed!" "ERROR"; exit 1)

--- a/builder/test/tests.sh
+++ b/builder/test/tests.sh
@@ -29,7 +29,6 @@ pigpiod -v
 i2cdetect -V
 butterfly -h
 espeak --version
-mjpg_streamer --version

 # ros stuff

--- a/clever/CMakeLists.txt
+++ b/clever/CMakeLists.txt
@@ -239,8 +239,3 @@ target_link_libraries(clever

 ## Add folders to be run by python nosetests
 # catkin_add_nosetests(test)
-
-if (CATKIN_ENABLE_TESTING)
-  find_package(rostest REQUIRED)
-  add_rostest(test/basic.test)
-endif()
--- a/clever/launch/aruco.launch
+++ b/clever/launch/aruco.launch
@@ -24,8 +24,6 @@
        <param name="known_tilt" value="map"/>
        <param name="frame_id" value="aruco_map_detected" if="$(arg aruco_vpe)"/>
        <param name="frame_id" value="aruco_map" unless="$(arg aruco_vpe)"/>
-        <param name="markers/frame_id" value="aruco_map"/>
-        <param name="markers/child_frame_id_prefix" value="aruco_map_"/>
    </node>

    <!-- vpe publisher from aruco markers -->
--- a/clever/launch/mavros.launch
+++ b/clever/launch/mavros.launch
@@ -56,7 +56,7 @@
    <node pkg="tf2_ros" type="static_transform_publisher" name="rangefinder_frame" args="0 0 -0.05 0 1.5707963268 0 base_link rangefinder"/>

    <!-- Copter visualization -->
-    <node name="visualization" pkg="mavros_extras" type="visualization" if="$(arg viz)">
+    <node name="copter_visualization" pkg="mavros_extras" type="copter_visualization" if="$(arg viz)">
        <remap to="mavros/local_position/pose" from="local_position"/>
        <remap to="mavros/setpoint_position/local" from="local_setpoint"/>
        <param name="fixed_frame_id" value="map"/>
--- a/clever/package.xml
+++ b/clever/package.xml
@@ -30,13 +30,13 @@
  <depend>nodelet</depend>
  <depend>mavros</depend>
  <depend>mavros_extras</depend>
+  <depend>lxml</depend>
  <depend>cv_camera</depend>
  <depend>cv_bridge</depend>
  <depend>opencv3</depend>
+  <depend>mjpg-streamer</depend>
  <depend>rosbridge_server</depend>
  <depend>web_video_server</depend>
-  <depend>tf2_web_republisher</depend>
-  <depend>python-lxml</depend>
  <exec_depend>python-pymavlink</exec_depend>
  <!-- Use test_depend for packages you need only for testing: -->
  <!--   <test_depend>gtest</test_depend> -->
--- a/clever/src/simple_offboard.cpp
+++ b/clever/src/simple_offboard.cpp
@@ -123,7 +123,7 @@ TwistStamped velocity;
 NavSatFix global_position;
 BatteryState battery;

-// Common subscriber callback template that stores message to the variable
+// Common subcriber callback template that stores message to the variable
 template<typename T, T& STORAGE>
 void handleMessage(const T& msg)
 {
--- a/clever/test/basic.py
+++ b/clever/test/basic.py
@@ -1,29 +0,0 @@
-#!/usr/bin/env python
-import rospy
-import pytest
-from mavros_msgs.msg import State
-
-@pytest.fixture()
-def node():
-    return rospy.init_node('clever_test', anonymous=True)
-
-def test_state(node):
-    state = rospy.wait_for_message('mavros/state', State, timeout=10)
-    assert state.connected == False
-    assert state.armed == False
-    assert state.guided == False
-    assert state.mode == ''
-
-def test_simple_offboard_services_available():
-    rospy.wait_for_service('get_telemetry', timeout=5)
-    rospy.wait_for_service('navigate', timeout=5)
-    rospy.wait_for_service('navigate_global', timeout=5)
-    rospy.wait_for_service('set_position', timeout=5)
-    rospy.wait_for_service('set_velocity', timeout=5)
-    rospy.wait_for_service('set_attitude', timeout=5)
-    rospy.wait_for_service('set_rates', timeout=5)
-    rospy.wait_for_service('land', timeout=5)
-
-def test_web_video_server(node):
-    import urllib2
-    urllib2.urlopen("http://localhost:8080").read()
--- a/clever/test/basic.test
+++ b/clever/test/basic.test
@@ -1,37 +0,0 @@
-<launch>
-    <!-- Verify all the required nodes basically work -->
-
-    <node pkg="mavros" type="mavros_node" name="mavros" required="true" output="screen">
-        <param name="fcu_url" value="udp://@127.0.1:14557"/>
-        <rosparam command="load" file="$(find clever)/launch/mavros_config.yaml"/>
-    </node>
-
-    <node name="visualization" pkg="mavros_extras" type="visualization" required="true">
-        <remap to="mavros/local_position/pose" from="local_position"/>
-        <remap to="mavros/setpoint_position/local" from="local_setpoint"/>
-        <param name="fixed_frame_id" value="map"/>
-        <param name="child_frame_id" value="base_link"/>
-        <param name="marker_scale" value="1"/>
-        <param name="max_track_size" value="20"/>
-        <param name="num_rotors" value="4"/>
-    </node>
-
-    <node name="web_video_server" pkg="web_video_server" type="web_video_server" required="true" output="screen">
-        <param name="default_stream_type" value="ros_compressed"/>
-        <param name="publish_rate" value="1.0"/>
-    </node>
-
-    <node pkg="tf2_ros" type="static_transform_publisher" name="map_flipped_frame" args="0 0 0 3.1415926 3.1415926 0 map map_flipped" required="true"/>
-
-    <node name="simple_offboard" pkg="clever" type="simple_offboard" required="true" output="screen">
-        <param name="reference_frames/body" value="map"/>
-        <param name="reference_frames/base_link" value="map"/>
-    </node>
-
-    <node name="tf2_web_republisher" pkg="tf2_web_republisher" type="tf2_web_republisher" required="true"/>
-
-    <node name="rc" pkg="clever" type="rc" required="true" output="screen"/>
-
-    <param name="test_module" value="$(find clever)/test/basic.py"/>
-    <test test-name="basic_test" pkg="ros_pytest" type="ros_pytest_runner"/>
-</launch>
--- a/docs/assets/cam_calib1.png
+++ b/docs/assets/cam_calib1.png
--- a/docs/assets/cam_calib2.png
+++ b/docs/assets/cam_calib2.png
--- a/docs/assets/cam_calib3.png
+++ b/docs/assets/cam_calib3.png
--- a/docs/assets/cam_calib4.png
+++ b/docs/assets/cam_calib4.png
--- a/docs/ru/aruco_map.md
+++ b/docs/ru/aruco_map.md
@@ -55,7 +55,7 @@ rosrun aruco_pose genmap.py length x y dist_x dist_y first > ~/catkin_ws/src/cle
 rosrun aruco_pose genmap.py 0.33 2 4 1 1 0 > ~/catkin_ws/src/clever/aruco_pose/map/test_map.txt
 ```

-Также, можно создать карту в специальном [конструкторе](arucogenmap.md).
+Также, можно создать карту в специальном [конструкторе](arucogenmap.md)

 ### Проверка

--- a/docs/ru/calibration.md
+++ b/docs/ru/calibration.md
@@ -1,99 +1,79 @@
 # Калибровка камеры

-Для точной работы систем компьютерного зрения (например, для навигации по ArUco-маркерам) используемая камера должна быть откалибрована.  
+Компьютерное зрение получает все более широкое распространение. Зачастую, алгоритмы компьютерного зрения работают неточно, получая искаженное изображение с камеры, что особенно характерно для fisheye-камер.

-![distorted](../assets/img1.jpg)
+![img](../assets/img1.jpg)

 > Изображение "скруглено" ближе к краям.
-Какой-либо алгоритм компьютерного зрения будет воспринимать информацию с этой картинки неправильно.

-## Установка приложения
+Какой-либо алгоритм компьютерного зрения будет воспринимать информацию с этой картинки неправильно. Для устранения подобных искажений камера, получающая изображения, должна быть откалибрована в соответствии со своими особенностями.
+
+## Установка скрипта

 Для начала, необходимо установить необходимые библиотеки:

-```bash
-pip install numpy  
-pip install opencv-python  
-pip install pyyaml  
-pip install urllib2
-pip install flask, flask-wtf
+```
+pip install numpy
+pip install opencv-python
+pip install glob
+pip install pyyaml
+pip install urllib.request
 ```

-Затем скачиваем исходный код из репозитория и проводим установку:
+Затем скачиваем скрипт из репозитория:

 ```bash
-git clone https://github.com/tinderad/calibration_web_2.7.git
-cd calibration_web_2.7.git
+git clone https://github.com/tinderad/clever_cam_calibration.git
+```
+
+Переходим в скачанную папку и устанавливаем скрипт:
+
+```bash
+cd clever_cam_calibration
 sudo python setup.py build
 sudo python setup.py install
 ```

+Если вы используете Windows, тогда скачайте архив из [репозитория](https://github.com/tinderad/clever_cam_calibration/archive/master.zip), распакуйте его и установите:
+
+```bash
+cd path\to\archive\clever_cam_calibration\
+python setup.py build
+python setup.py install
+```
+
+> path\to\archive - путь до распакованного архива.
+
 ## Подготовка к калибровке

-Вам необходимо подготовить калибровочную мишень. Она представляет собой «шахматную доску». Файл можно взять [отсюда](https://www.oreilly.com/library/view/learning-opencv-3/9781491937983/assets/lcv3_ac01.png).  
-Наклейте распечатанную мишень на любую твердую поверхность. Посчитайте количество пересечений в длину и в ширину доски, измерьте размер клетки (в мм), как указано на изображении.
+Вам необходимо подготовить калибровочную мишень. Она представляет собой «шахматную доску». Файл можно взять [отсюда](https://www.oreilly.com/library/view/learning-opencv-3/9781491937983/assets/lcv3_ac01.png).
+Наклейте распечатанную мишень на любую твердую поверхность. Посчитайте количество пересечений в длину и в ширину доски, измерьте размер клетки (в мм).

-![asd](../assets/chessboard.jpg)
+![img](../assets/chessboard.jpg)

-Включите Клевер и подключитесь к его Wifi.
+Включите Клевер и подключитесь к его Wi-Fi.

-> Перейдите на _192.168.11.1:8080_ и проверьте, получает ли компьютер изображения из топика _image_raw_.
+> Перейдите на 192.168.11.1:8080 и проверьте, получает ли компьютер изображения из топика image_raw.

 ## Калибровка

-Подключитесь к Клеверу по протоколу SSH (например, при помощи PuTTY).
+Запустите скрипт **_calibrate_cam_**:

-Запустите приложение:
+**Windows:**

-```bash
->cd calibration_web_2.7/ccc_server
->python app.py
+ ```bash
+>path\to\python\Scripts\calibrate_cam.exe
 ```

-Далее вам необходимо на компьютере открыть в браузере страницу по адресу _192.168.11.1:8081_
+> path\to\Python - путь до директории Python

-> Порт можно настроить в файле _ccc_server/config.py_.
-
-На открытой странице необходимо ввести параметры калибровочной мишени: количество перекрестий в длину и ширину, длину ребра квадрата. Для начала калибровки нажмите кнопку  **_Start Calibration_**.
-
-![asd](../assets/cam_calib1.png)
-
-На следующей странице при помощи кнопки **_Catch photo_** можно делать фотографии калибровочной мишени.
-
-![asd](../assets/cam_calib2.png)
-
-Если программа нашла на изображении указанную мишень, откроется страница, на которой вам необходимо подтвердить корректность найденных перекрестий.
-
-![asd](../assets/cam_calib3.png)
-
-Если перекрестия были распознаныы правильно, нажмите на клавишу **_Add_**, и перейдите к получению новых фотографий. В противном же случае, ели перекристия были распознаны некорректно, пропустите данную фотографию при помощи клавиши **_Skip_**.
-
->В большинстве случаев найденные углы будут подсвечиваться разными цветами, но иногда подсветка будет становиться красной. это происходит в том случае, если углы распознаны, но неточно.
-
-Чтобы откалибровать камеру, вам требуется сделать как минимум 25 фото шахматной доски с различных ракурсов. После преодоления данного порога появится кнопка **_Finish_**, по нажатию на которую начнется генерация калибровочного файла.
-
->Это может занять некоторое время.
-
-На открывшейся странице выведется информация о результате калибровки: имя файла и re-projection error.
->re-projection error - отклонение от стандартной математической модели. Чем эта величина меньше, тем точнее проведена калибровка.
-
-![asd](../assets/cam_calib4.png)
-
-Программа обработает все полученные фотографии, и создаст **_.yaml_** файл в нынешней директории. При помощи этого файла можно будет выравнивать искажения на изображениях, полученных с этой камеры.
-
-> Если вы поменяете разрешение получаемого изображения, вам нужно будет снова калибровать камеру.
-
-## Предыдущая версия
-
-Также вы можете воспользоваться предыдущей версией программы, не имеющей web-интерфейса.
-
-Запустите скрипт **_calibrate_cam_**:
+**Linux:**

 ```bash
 >calibrate_cam
 ```

-Задайте параметры мишени:
+Задайте параметры доски:

 ```bash
 >calibrate_cam
@@ -115,6 +95,8 @@ help, catch (key: Enter), delete, restart, stop, finish

 Чтобы откалибровать камеру, вам требуется сделать как минимум 25 фото шахматной доски с различных ракурсов.

+![img](../assets/calibration.jpg)
+
 Чтобы сделать фото, введите команду **_catch_**.

 ```bash
@@ -143,10 +125,10 @@ Calibration successful!

 **Калибровка по существующим изображениям:**

-Если же у вас уже есть изображения, то вы можете откалибровать камеру по ним при помощи скрипта **_calibrate_from_dir_**.
+Если же у вас уже есть изображения, то вы можете откалибровать камеру по ним при помощи скрипта **_calibrate_cam_ex_**.

 ```bash
->calibrate_from_dir
+>calibrate_cam_ex
 ```

 Указываем характеристики мишени, а так же путь до папки с изображениями:
@@ -163,31 +145,32 @@ Path:  # Путь до папки с изображениями

 Программа обработает все полученные фотографии, и создаст файл **_camera_info_****_._****_yaml_** в нынешней директории. При помощи этого файла можно будет выравнивать искажения на изображениях, полученных с этой камеры.

+> Если вы поменяете разрешение получаемого изображения, вам нужно будет снова калибровать камеру.
+
 ## Исправление искажений

-За получение исправленного изображения отвечает функция
-**clever_cam_calibration._get_undistorted_image(cv2_image, camera_info)_**:
+За получение исправленного изображения отвечает функция **_get_undistorted_image(cv2_image, camera_info)_**:

 * **_cv2_image_**: Закодированное в массив cv2 изображение.
 * **_camera_****___****_info_**: Путь до файла калибровки.

 Функция возвращает массив cv2, в котором закодировано исправленное изображение.

-> Если вы используете fisheye-камеру, поставляемую вместе с Клевером, то для обработки изображений разрешением 320x240 или 640x480 вы можете использовать уже существующие параметры калибровки. Для этого в качестве аргумента **_camera_info_** передайте параметры **_clever_cam_calibration.CLEVER_FISHEYE_CAM_320_** или **_clever_cam_calibration.CLEVER_FISHEYE_CAM_640_** соответственно.
+> Если вы используете fisheye-камеру, поставляемую вместе с Клевером, то для обработки изображений разрешением 320x240 или 640x480 вы можете использовать уже существующие параметры калибровки. Для этого в качестве аргумента **_camera_info_** передайте параметры **_clever_cam_calibration.clevercamcalib.CLEVER_FISHEYE_CAM_320_** или **_clever_cam_calibration.clevercamcalib.CLEVER_FISHEYE_CAM_640_** соответственно.

 ## Примеры работы

 Изначальные изображения:

-![asd](../assets/img1.jpg)
+![img](../assets/img1.jpg)

-![asd](../assets/img2.jpg)
+![img](../assets/img2.jpg)

 Иcправленные изображения:

-![asd](../assets/calibresult.jpg)
+![img](../assets/calibresult.jpg)

-![asd](../assets/calibresult1.jpg)
+![img](../assets/calibresult1.jpg)

 ## Пример использования

@@ -196,17 +179,17 @@ Path:  # Путь до папки с изображениями
 Данная программа получает изображения с камеры Клевера и выводит их на экран в исправленном виде, используя существующий калибровочный файл.

 ```python
-import clever_cam_calibration as ccc  
-import cv2  
-import urllib.request  
-import numpy as np  
-while True:  
-	req = urllib.request.urlopen('http://192.168.11.1:8080/snapshot?topic=/main_camera/image_raw')  
-    arr = np.asarray(bytearray(req.read()), dtype=np.uint8)  
-    image = cv2.imdecode(arr, -1)  
-    undistorted_img = ccc.get_undistorted_image(image, ccc.CLEVER_FISHEYE_CAM_640)  
-    cv2.imshow("undistort", undistorted_img)  
-    cv2.waitKey(33)  
+import clevercamcalib.clevercamcalib as ccc
+import cv2
+import urllib.request
+import numpy as np
+while True:
+	req = urllib.request.urlopen('http://192.168.11.1:8080/snapshot?topic=/main_camera/image_raw')
+    arr = np.asarray(bytearray(req.read()), dtype=np.uint8)
+    image = cv2.imdecode(arr, -1)
+    undistorted_img = ccc.get_undistorted_image(image, ccc.CLEVER_FISHEYE_CAM_640)
+    cv2.imshow("undistort", undistorted_img)
+    cv2.waitKey(33)
 cv2.destroyAllWindows()
 ```

@@ -215,11 +198,13 @@ cv2.destroyAllWindows()
 Чтобы применить параметры калибровки к системе ArUco-навигации, требуется перенести калибровочный .yaml файл на Raspberry Pi Клевера и инициализировать его.

 > Не забудьте подключиться к WiFI Клевера.
+
 Для передачи файла используется протокол SFTP. В данном примере используется программа WinSCP.

 Подключимся к Raspberry Pi по SFTP:

 > Пароль: _**raspberry**_
+
 ![img](../assets/wcp1.png)

 Нажимаем “Войти”. Переходим в _**/home/pi/catkin_ws/src/clever/clever/camera_info/**_ и копируем туда калибровочный .yaml файл:
@@ -240,4 +225,4 @@ cv2.destroyAllWindows()

 ![img](../assets/pty3.jpg)

-> Не забудьте изменить разрешение камеры в *main_camera.launch*.
+> Не забудьте изменить разрешение камеры.
--- a/docs/ru/network.md
+++ b/docs/ru/network.md
@@ -7,30 +7,28 @@ Wi-Fi адаптер на Raspberry Pi имеет два основных реж

 При использовании [образа для RPi](microsd_images.md) по умолчанию Wi-Fi адаптер работает в [режиме точки доступа](wifi.md).

-<!-- markdownlint-disable MD029 -->
-
 ## Изменение пароля или SSID (имени сети)

 1. Отредактируйте файл `/etc/wpa_supplicant/wpa_supplicant.conf` (используя [SSH-соединение](ssh.md)):

-  ```bash
-  sudo nano /etc/wpa_supplicant/wpa_supplicant.conf
-  ```
+    ```bash
+    sudo nano /etc/wpa_supplicant/wpa_supplicant.conf
+    ```

-  Измените значение параметра `ssid`, чтобы изменить название Wi-Fi сети, и параметра `psk`, чтобы изменить пароль. Например:
+    Измените значение параметра `ssid`, чтобы изменить название Wi-Fi сети, и параметра `psk`, чтобы изменить пароль. Например:

-  ```txt
-  network={
-      ssid="my-super-ssid"
-      psk="cleverwifi123"
-      mode=2
-      proto=RSN
-      key_mgmt=WPA-PSK
-      pairwise=CCMP
-      group=CCMP
-      auth_alg=OPEN
-  }
-  ```
+    ```txt
+    network={
+        ssid="my-super-ssid"
+        psk="cleverwifi123"
+        mode=2
+        proto=RSN
+        key_mgmt=WPA-PSK
+        pairwise=CCMP
+        group=CCMP
+        auth_alg=OPEN
+    }
+    ```

 2. Перезагрузите Raspberry Pi.

@@ -42,89 +40,87 @@ Wi-Fi адаптер на Raspberry Pi имеет два основных реж

 1. Выключите службу `dnsmasq`.

-  ```bash
-  sudo systemctl stop dnsmasq
-  sudo systemctl disable dnsmasq
-  ```
+    ```bash
+    sudo systemctl stop dnsmasq
+    sudo systemctl disable dnsmasq
+    ```

 2. Включите получение IP адреса на беспроводном интерфейсе DHCP клиентом. Для этого удалите из файла `/etc/dhcpcd.conf` строки:

-  ```conf
-  interface wlan0
-  static ip_address=192.168.11.1/24
-  ```
+    ```conf
+    interface wlan0
+    static ip_address=192.168.11.1/24
+    ```

 3. Настройте `wpa_supplicant` для подключения к существующей точке доступа. Для этого замените содержимое файла `/etc/wpa_supplicant/wpa_supplicant.conf` на:

-  ```
-  ctrl_interface=DIR=/var/run/wpa_supplicant GROUP=netdev
-  update_config=1
-  country=GB
+    ```
+    ctrl_interface=DIR=/var/run/wpa_supplicant GROUP=netdev
+    update_config=1
+    country=GB

-  network={
-      ssid="SSID"
-      psk="password"
-  }
-  ```
+    network={
+        ssid="SSID"
+        psk="password"
+    }
+    ```

-  где `SSID` – название сети, а `password` – пароль.
+    где `SSID` – название сети, а `password` – пароль.

 4. Перезапустите службу `dhcpcd`.

-  ```bash
-  sudo systemctl restart dhcpcd
-  ```
+    ```bash
+    sudo systemctl restart dhcpcd
+    ```

 ## Переключение адаптера в режим точки доступа

 1. Включите статический IP адрес на беспроводном интерфейсе. Для этого добавьте в файл `/etc/dhcpcd.conf` строки:

-  ```conf
-  interface wlan0
-  static ip_address=192.168.11.1/24
-  ```
+    ```conf
+    interface wlan0
+    static ip_address=192.168.11.1/24
+    ```

 2. Настроите `wpa_supplicant` на работу в режиме точки доступа. Для этого замените содержимое файла `/etc/wpa_supplicant/wpa_supplicant.conf` на:

-  ```
-  ctrl_interface=DIR=/var/run/wpa_supplicant GROUP=netdev
-  update_config=1
-  country=GB
+    ```
+    ctrl_interface=DIR=/var/run/wpa_supplicant GROUP=netdev
+    update_config=1
+    country=GB

-  network={
-      ssid="CLEVER-1234"
-      psk="cleverwifi"
-      mode=2
-      proto=RSN
-      key_mgmt=WPA-PSK
-      pairwise=CCMP
-      group=CCMP
-      auth_alg=OPEN
-  }
-  ```
+    network={
+        ssid="CLEVER-1234"
+        psk="cleverwifi"
+        mode=2
+        proto=RSN
+        key_mgmt=WPA-PSK
+        pairwise=CCMP
+        group=CCMP
+        auth_alg=OPEN
+    }
+    ```

-  где `CLEVER-1234` – название сети, а `cleverwifi` – пароль.
+    где `CLEVER-1234` – название сети, а `cleverwifi` – пароль.

 3. Перезагрузите `systemd`.

-  ```bash
-  sudo systemctl daemon-reload
-  ```
+    ```bash
+    sudo systemctl daemon-reload
+    ```

 4. Включите службу `dnsmasq`.

-  ```bash
-  sudo systemctl enable dnsmasq
-  sudo systemctl start dnsmasq
-  ```
+    ```bash
+    sudo systemctl enable dnsmasq
+    sudo systemctl start dnsmasq
+    ```

 5. Перезагрузите службу `dhcpcd`.

-  ```bash
-  sudo systemctl restart dhcpcd
-  ```
-
-<!-- markdownlint-enable MD029 -->
+    ```bash
+    sudo systemctl restart dhcpcd
+    ```

 ___