docs: some info on optical flow troubleshooting

Fixed broken links

.gitignore

@@ -1,2 +1,3 @@
-/deploy/ros_lib/
 *.pyc
+*.DS_Store
+/images

.travis.yml

@@ -0,0 +1,35 @@
+sudo: required
+language: generic
+services:
+  - docker
+env:
+  global:
+    - 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"
+git:
+  depth: 1
+before_script:
+  - docker pull ${DOCKER}
+script:
+  - docker run --privileged --rm -v /dev:/dev -v $(pwd):/builder/repo -e TRAVIS_TAG="${TRAVIS_TAG}" ${DOCKER}
+before_deploy:
+  # Set up git user name and tag this commit
+  - git config --local user.name "urpylka"
+  - git config --local user.email "urpylka@gmail.com"
+  - sudo chmod -R 777 *
+  - cd images && zip ${IMAGE_NAME}.zip ${IMAGE_NAME}
+deploy:
+  provider: releases
+  api_key: ${GITHUB_OAUTH_TOKEN}
+  file: ${IMAGE_NAME}.zip
+  skip_cleanup: true
+  on:
+    tags: true
+
+# More info there
+# https://github.com/travis-ci/travis-ci/issues/6893
+# https://docs.travis-ci.com/user/customizing-the-build/
+# https://docs.travis-ci.com/user/deployment/releases
+# https://docs.travis-ci.com/user/environment-variables/

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](http://builder.coex.space/job/CopterExpress---clever/badge/icon)](http://builder.coex.space/job/CopterExpress---clever/)
+[![Build Status](https://travis-ci.org/urpylka/clever.svg?branch=master)](https://travis-ci.org/urpylka/clever)
 
 Image includes:
 

_config.yml

@@ -1,2 +0,0 @@
-theme: jekyll-theme-cayman
-tagline: Конструктор программируемого квадрокоптера

aruco_pose/CMakeLists.txt

@@ -16,11 +16,15 @@ find_package(catkin REQUIRED COMPONENTS
   image_transport
   cv_bridge
   tf
-  #tf2
-  #tf2_ros
-  #aruco_msgs
+  tf2
+  tf2_ros
+  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(
-#  FILES
-#  Marker.msg
-#  MarkerArray.msg
-#)
+add_message_files(
+  FILES
+  Point2D.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(
-#  DEPENDENCIES
-#  std_msgs  # Or other packages containing msgs
-#)
+generate_messages(
+  DEPENDENCIES
+  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}
-  src/aruco_pose.cpp
+add_library(aruco_pose
+  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(${PROJECT_NAME}
+target_link_libraries(aruco_pose
   ${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,61 +2,43 @@
 <package>
   <name>aruco_pose</name>
   <version>0.0.0</version>
-  <description>ArUco maps precise pose estimation nodelet</description>
+  <description>Positioning by ArUco markers</description>
 
-  <!-- One maintainer tag required, multiple allowed, one person per tag -->
-  <!-- Example:  -->
-  <!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
   <maintainer email="okalachev@gmail.com">Oleg Kalachev</maintainer>
+  <license>MIT</license>
 
+  <!--url type="website">http://wiki.ros.org/aruco_pose</url-->
+  <author email="okalachev@gmail.com">Oleg Kalachev</author>
+  <author email="urpylka@gmail.com">Artem Smirnov</author>
 
-  <!-- One license tag required, multiple allowed, one license per tag -->
-  <!-- Commonly used license strings: -->
-  <!--   BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->
-  <license>TODO</license>
-
-
-  <!-- Url tags are optional, but multiple are allowed, one per tag -->
-  <!-- Optional attribute type can be: website, bugtracker, or repository -->
-  <!-- Example: -->
-  <!-- <url type="website">http://wiki.ros.org/aruco_pose</url> -->
-
-
-  <!-- Author tags are optional, multiple are allowed, one per tag -->
-  <!-- Authors do not have to be maintainers, but could be -->
-  <!-- Example: -->
-  <!-- <author email="jane.doe@example.com">Jane Doe</author> -->
-
-
-  <!-- The *_depend tags are used to specify dependencies -->
-  <!-- Dependencies can be catkin packages or system dependencies -->
-  <!-- Examples: -->
-  <!-- Use build_depend for packages you need at compile time: -->
-  <!--   <build_depend>message_generation</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>message_runtime</run_depend> -->
-  <!-- Use test_depend for packages you need only for testing: -->
-  <!--   <test_depend>gtest</test_depend> -->
   <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>cv_bridge</build_depend>
-  <build_depend>tf</build_depend>
-
+  <build_depend>std_msgs</build_depend>
   <run_depend>nodelet</run_depend>
   <run_depend>roscpp</run_depend>
   <run_depend>image_transport</run_depend>
   <run_depend>cv_bridge</run_depend>
-  <build_depend>tf</build_depend>
+  <run_depend>message_runtime</run_depend>
+  <run_depend>std_msgs</run_depend>
+  <run_depend>tf</run_depend>
+  <run_depend>tf2</run_depend>
+  <run_depend>tf2_ros</run_depend>
+  <run_depend>tf2_geometry_msgs</run_depend>
 
   <!-- The export tag contains other, unspecified, tags -->
   <export>
-        <nodelet plugin="${prefix}/nodelet_plugins.xml" />
+    <nodelet plugin="${prefix}/nodelet_plugins.xml" />
     <!-- Other tools can request additional information be placed here -->
-
   </export>
 </package>

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;
+}

builder/assets/clever.service

@@ -4,7 +4,7 @@ Requires=roscore.service
 After=roscore.service
 
 [Service]
-EnvironmentFile=/home/pi/catkin_ws/src/clever/deploy/roscore.env
+EnvironmentFile=/lib/systemd/system/roscore.env
 ExecStart=/opt/ros/kinetic/bin/roslaunch clever clever.launch --wait
 Restart=on-abort
 

builder/assets/hardware_setup.sh

@@ -1,39 +1,62 @@
-#!/bin/bash
-
-# Exit immidiately on non-zero result
-set -e
+#! /usr/bin/env bash
 
 #
-# Script for image configure
-# @urpylka Artem Smirnov
+# Script for build the image. Used builder script of the target repo
+# For build: docker run --privileged -it --rm -v /dev:/dev -v $(pwd):/builder/repo smirart/builder
 #
+# Copyright (C) 2018 Copter Express Technologies
+#
+# Author: Artem Smirnov <urpylka@gmail.com>
+#
+
+set -e # Exit immidiately on non-zero result
+
+echo_stamp() {
+  # TEMPLATE: echo_stamp <TEXT> <TYPE>
+  # TYPE: SUCCESS, ERROR, INFO
+
+  # More info there https://www.shellhacks.com/ru/bash-colors/
+
+  TEXT="$(date '+[%Y-%m-%d %H:%M:%S]') $1"
+  TEXT="\e[1m$TEXT\e[0m" # BOLD
+
+  case "$2" in
+    SUCCESS)
+    TEXT="\e[32m${TEXT}\e[0m";; # GREEN
+    ERROR)
+    TEXT="\e[31m${TEXT}\e[0m";; # RED
+    *)
+    TEXT="\e[34m${TEXT}\e[0m";; # BLUE
+  esac
+  echo -e ${TEXT}
+}
 
 ##################################################
 # Configure hardware interfaces
 ##################################################
 
 # 1. Enable sshd
-echo -e "\033[0;31m\033[1m$(date) | #1 Turn on sshd\033[0m\033[0m"
+echo_stamp "#1 Turn on sshd"
 touch /boot/ssh
 # /usr/bin/raspi-config nonint do_ssh 0
 
 # 2. Enable GPIO
-echo -e "\033[0;31m\033[1m$(date) | #2 GPIO enabled by default\033[0m\033[0m"
+echo_stamp "#2 GPIO enabled by default"
 
 # 3. Enable I2C
-echo -e "\033[0;31m\033[1m$(date) | #3 Turn on I2C\033[0m\033[0m"
+echo_stamp "#3 Turn on I2C"
 /usr/bin/raspi-config nonint do_i2c 0
 
 # 4. Enable SPI
-echo -e "\033[0;31m\033[1m$(date) | #4 Turn on SPI\033[0m\033[0m"
+echo_stamp "#4 Turn on SPI"
 /usr/bin/raspi-config nonint do_spi 0
 
 # 5. Enable raspicam
-echo -e "\033[0;31m\033[1m$(date) | #5 Turn on raspicam\033[0m\033[0m"
+echo_stamp "#5 Turn on raspicam"
 /usr/bin/raspi-config nonint do_camera 0
 
 # 6. Enable hardware UART
-echo -e "\033[0;31m\033[1m$(date) | #6 Turn on UART\033[0m\033[0m"
+echo_stamp "#6 Turn on UART"
 # Temporary solution
 # https://github.com/RPi-Distro/raspi-config/pull/75
 /usr/bin/raspi-config nonint do_serial 1
@@ -47,9 +70,9 @@ systemctl disable hciuart.service
 
 # 7. Enable V4L driver http://robocraft.ru/blog/electronics/3158.html
 #echo "bcm2835-v4l2" >> /etc/modules
-echo -e "\033[0;31m\033[1m$(date) | #7 Turn on v4l2 driver\033[0m\033[0m"
+echo_stamp "#7 Turn on v4l2 driver"
 if ! grep -q "^bcm2835-v4l2" /etc/modules;
 then printf "bcm2835-v4l2\n" >> /etc/modules
 fi
 
-echo -e "\033[0;31m\033[1m$(date) | End of configure hardware interfaces\033[0m\033[0m"
+echo_stamp "#8 End of configure hardware interfaces"

builder/assets/init_rpi.sh

@@ -0,0 +1,43 @@
+#! /usr/bin/env bash
+
+#
+# Script for build the image. Used builder script of the target repo
+# For build: docker run --privileged -it --rm -v /dev:/dev -v $(pwd):/builder/repo smirart/builder
+#
+# Copyright (C) 2018 Copter Express Technologies
+#
+# Author: Artem Smirnov <urpylka@gmail.com>
+#
+
+set -e # Exit immidiately on non-zero result
+
+echo_stamp() {
+  # TEMPLATE: echo_stamp <TEXT> <TYPE>
+  # TYPE: SUCCESS, ERROR, INFO
+
+  # More info there https://www.shellhacks.com/ru/bash-colors/
+
+  TEXT="$(date '+[%Y-%m-%d %H:%M:%S]') $1"
+  TEXT="\e[1m$TEXT\e[0m" # BOLD
+
+  case "$2" in
+    SUCCESS)
+    TEXT="\e[32m${TEXT}\e[0m";; # GREEN
+    ERROR)
+    TEXT="\e[31m${TEXT}\e[0m";; # RED
+    *)
+    TEXT="\e[34m${TEXT}\e[0m";; # BLUE
+  esac
+  echo -e ${TEXT}
+}
+
+echo_stamp "Rename SSID"
+sudo sed -i.OLD "s/CLEVER/CLEVER-$(head -c 100 /dev/urandom | xxd -ps -c 100 | sed -e 's/[^0-9]//g' | cut -c 1-4)/g" /etc/wpa_supplicant/wpa_supplicant.conf
+
+echo_stamp "Harware setup"
+/root/hardware_setup.sh
+
+echo_stamp "Remove init scripts"
+rm /root/init_rpi.sh /root/hardware_setup.sh
+
+echo_stamp "End of initialization of the image"

builder/assets/kinetic-ros-clever.rosinstall

@@ -418,10 +418,6 @@
     local-name: nodelet_core/nodelet
     uri: https://github.com/ros-gbp/nodelet_core-release/archive/release/kinetic/nodelet/1.9.14-0.tar.gz
     version: nodelet_core-release-release-kinetic-nodelet-1.9.14-0
-- tar:
-    local-name: opencv3
-    uri: https://github.com/ros-gbp/opencv3-release/archive/release/kinetic/opencv3/3.3.1-5.tar.gz
-    version: opencv3-release-release-kinetic-opencv3-3.3.1-5
 - tar:
     local-name: orocos_kinematics_dynamics/orocos_kdl
     uri: https://github.com/smits/orocos-kdl-release/archive/release/kinetic/orocos_kdl/1.3.1-0.tar.gz

builder/assets/kinetic-rosdep-clever.yaml

@@ -0,0 +1,528 @@
+actionlib:
+  debian:
+    stretch: [ros-kinetic-actionlib]
+actionlib_msgs:
+  debian:
+    stretch: [ros-kinetic-actionlib-msgs]
+angles:
+  debian:
+    stretch: [ros-kinetic-angles]
+async_web_server_cpp:
+  debian:
+    stretch: [ros-kinetic-async-web-server-cpp]
+bond:
+  debian:
+    stretch: [ros-kinetic-bond]
+bondcpp:
+  debian:
+    stretch: [ros-kinetic-bondcpp]
+bondpy:
+  debian:
+    stretch: [ros-kinetic-bondpy]
+camera_calibration_parsers:
+  debian:
+    stretch: [ros-kinetic-camera-calibration-parsers]
+camera_info_manager:
+  debian:
+    stretch: [ros-kinetic-camera-info-manager]
+catkin:
+  debian:
+    stretch: [ros-kinetic-catkin]
+class_loader:
+  debian:
+    stretch: [ros-kinetic-class-loader]
+cmake_modules:
+  debian:
+    stretch: [ros-kinetic-cmake-modules]
+cpp_common:
+  debian:
+    stretch: [ros-kinetic-cpp-common]
+cv_bridge:
+  debian:
+    stretch: [ros-kinetic-cv-bridge]
+cv_camera:
+  debian:
+    stretch: [ros-kinetic-cv-camera=0.3.1-0stretch]
+diagnostic_msgs:
+  debian:
+    stretch: [ros-kinetic-diagnostic-msgs]
+diagnostic_updater:
+  debian:
+    stretch: [ros-kinetic-diagnostic-updater]
+eigen_conversions:
+  debian:
+    stretch: [ros-kinetic-eigen-conversions]
+gencpp:
+  debian:
+    stretch: [ros-kinetic-gencpp]
+geneus:
+  debian:
+    stretch: [ros-kinetic-geneus]
+genlisp:
+  debian:
+    stretch: [ros-kinetic-genlisp]
+genmsg:
+  debian:
+    stretch: [ros-kinetic-genmsg]
+gennodejs:
+  debian:
+    stretch: [ros-kinetic-gennodejs]
+genpy:
+  debian:
+    stretch: [ros-kinetic-genpy]
+geographic_msgs:
+  debian:
+    stretch: [ros-kinetic-geographic-msgs]
+geometry_msgs:
+  debian:
+    stretch: [ros-kinetic-geometry-msgs]
+image_transport:
+  debian:
+    stretch: [ros-kinetic-image-transport]
+libmavconn:
+  debian:
+    stretch: [ros-kinetic-libmavconn]
+lxml:
+  debian:
+    stretch: [python-lxml=3.7.1-1]
+mavlink:
+  debian:
+    stretch: [ros-kinetic-mavlink]
+mavros:
+  debian:
+    stretch: [ros-kinetic-mavros]
+mavros_msgs:
+  debian:
+    stretch: [ros-kinetic-mavros-msgs]
+mavros_extras:
+  debian:
+    stretch: [ros-kinetic-mavros-extras]
+message_filters:
+  debian:
+    stretch: [ros-kinetic-message-filters]
+message_generation:
+  debian:
+    stretch: [ros-kinetic-message-generation]
+message_runtime:
+  debian:
+    stretch: [ros-kinetic-message-runtime]
+mjpg-streamer:
+  debian:
+    stretch: [mjpg-streamer=2.0]
+mk:
+  debian:
+    stretch: [ros-kinetic-mk]
+nav_msgs:
+  debian:
+    stretch: [ros-kinetic-nav-msgs]
+nodelet:
+  debian:
+    stretch: [ros-kinetic-nodelet]
+opencv3:
+  debian:
+    stretch: [ros-kinetic-opencv3]
+orocos_kdl:
+  debian:
+    stretch: [ros-kinetic-orocos-kdl]
+pluginlib:
+  debian:
+    stretch: [ros-kinetic-pluginlib]
+python_orocos_kdl:
+  debian:
+    stretch: [ros-kinetic-python-orocos-kdl]
+ros:
+  debian:
+    stretch: [ros-kinetic-ros]
+ros_comm:
+  debian:
+    stretch: [ros-kinetic-ros-comm]
+ros_environment:
+  debian:
+    stretch: [ros-kinetic-ros-environment]
+rosapi:
+  debian:
+    stretch: [ros-kinetic-rosapi]
+rosauth:
+  debian:
+    stretch: [ros-kinetic-rosauth]
+rosbag:
+  debian:
+    stretch: [ros-kinetic-rosbag]
+rosbag_migration_rule:
+  debian:
+    stretch: [ros-kinetic-rosbag-migration-rule]
+rosbag_storage:
+  debian:
+    stretch: [ros-kinetic-rosbag-storage]
+rosbash:
+  debian:
+    stretch: [ros-kinetic-rosbash]
+rosboost_cfg:
+  debian:
+    stretch: [ros-kinetic-rosboost-cfg]
+rosbridge_library:
+  debian:
+    stretch: [ros-kinetic-rosbridge-library]
+rosbridge_server:
+  debian:
+    stretch: [ros-kinetic-rosbridge-server]
+rosbuild:
+  debian:
+    stretch: [ros-kinetic-rosbuild]
+rosclean:
+  debian:
+    stretch: [ros-kinetic-rosclean]
+rosconsole:
+  debian:
+    stretch: [ros-kinetic-rosconsole]
+rosconsole_bridge:
+  debian:
+    stretch: [ros-kinetic-rosconsole-bridge]
+roscpp:
+  debian:
+    stretch: [ros-kinetic-roscpp]
+roscpp_serialization:
+  debian:
+    stretch: [ros-kinetic-roscpp-serialization]
+roscpp_traits:
+  debian:
+    stretch: [ros-kinetic-roscpp-traits]
+roscreate:
+  debian:
+    stretch: [ros-kinetic-roscreate]
+rosgraph:
+  debian:
+    stretch: [ros-kinetic-rosgraph]
+rosgraph_msgs:
+  debian:
+    stretch: [ros-kinetic-rosgraph-msgs]
+roslang:
+  debian:
+    stretch: [ros-kinetic-roslang]
+roslaunch:
+  debian:
+    stretch: [ros-kinetic-roslaunch]
+roslib:
+  debian:
+    stretch: [ros-kinetic-roslib]
+roslint:
+  debian:
+    stretch: [ros-kinetic-roslint]
+roslisp:
+  debian:
+    stretch: [ros-kinetic-roslisp]
+roslz4:
+  debian:
+    stretch: [ros-kinetic-roslz4]
+rosmake:
+  debian:
+    stretch: [ros-kinetic-rosmake]
+rosmaster:
+  debian:
+    stretch: [ros-kinetic-rosmaster]
+rosmsg:
+  debian:
+    stretch: [ros-kinetic-rosmsg]
+rosnode:
+  debian:
+    stretch: [ros-kinetic-rosnode]
+rosout:
+  debian:
+    stretch: [ros-kinetic-rosout]
+rospack:
+  debian:
+    stretch: [ros-kinetic-rospack]
+rosparam:
+  debian:
+    stretch: [ros-kinetic-rosparam]
+rospy:
+  debian:
+    stretch: [ros-kinetic-rospy]
+rospy_tutorials:
+  debian:
+    stretch: [ros-kinetic-rospy-tutorials]
+rosserial_client:
+  debian:
+    stretch: [ros-kinetic-rosserial-client]
+rosserial_msgs:
+  debian:
+    stretch: [ros-kinetic-rosserial-msgs]
+rosserial_python:
+  debian:
+    stretch: [ros-kinetic-rosserial-python]
+rosservice:
+  debian:
+    stretch: [ros-kinetic-rosservice]
+rostest:
+  debian:
+    stretch: [ros-kinetic-rostest]
+rostime:
+  debian:
+    stretch: [ros-kinetic-rostime]
+rostopic:
+  debian:
+    stretch: [ros-kinetic-rostopic]
+rosunit:
+  debian:
+    stretch: [ros-kinetic-rosunit]
+roswtf:
+  debian:
+    stretch: [ros-kinetic-roswtf]
+sensor_msgs:
+  debian:
+    stretch: [ros-kinetic-sensor-msgs]
+smclib:
+  debian:
+    stretch: [ros-kinetic-smclib]
+std_msgs:
+  debian:
+    stretch: [ros-kinetic-std-msgs]
+std_srvs:
+  debian:
+    stretch: [ros-kinetic-std-srvs]
+stereo_msgs:
+  debian:
+    stretch: [ros-kinetic-stereo-msgs]
+tf2:
+  debian:
+    stretch: [ros-kinetic-tf2]
+tf2_bullet:
+  debian:
+    stretch: [ros-kinetic-tf2-bullet]
+tf2_eigen:
+  debian:
+    stretch: [ros-kinetic-tf2-eigen]
+tf2_geometry_msgs:
+  debian:
+    stretch: [ros-kinetic-tf2-geometry-msgs]
+tf2_kdl:
+  debian:
+    stretch: [ros-kinetic-tf2-kdl]
+tf2_msgs:
+  debian:
+    stretch: [ros-kinetic-tf2-msgs]
+tf2_py:
+  debian:
+    stretch: [ros-kinetic-tf2-py]
+tf2_ros:
+  debian:
+    stretch: [ros-kinetic-tf2-ros]
+tf2_sensor_msgs:
+  debian:
+    stretch: [ros-kinetic-tf2-sensor-msgs]
+tf2_tools:
+  debian:
+    stretch: [ros-kinetic-tf2-tools]
+tf:
+  debian:
+    stretch: [ros-kinetic-tf]
+topic_tools:
+  debian:
+    stretch: [ros-kinetic-topic-tools]
+trajectory_msgs:
+  debian:
+    stretch: [ros-kinetic-trajectory-msgs]
+urdf:
+  debian:
+    stretch: [ros-kinetic-urdf]
+urdf_parser_plugin:
+  debian:
+    stretch: [ros-kinetic-urdf-parser-plugin]
+uuid_msgs:
+  debian:
+    stretch: [ros-kinetic-uuid-msgs]
+visualization_msgs:
+  debian:
+    stretch: [ros-kinetic-visualization-msgs]
+xmlrpcpp:
+  debian:
+    stretch: [ros-kinetic-xmlrpcpp]
+boost:
+  debian:
+    stretch: [libboost-all-dev]
+bullet:
+  debian:
+    stretch: [libbullet-dev]
+bzip2:
+  debian:
+    stretch: [libbz2-dev]
+cmake:
+  debian:
+    stretch: [cmake]
+cppunit:
+  debian:
+    stretch: [libcppunit-dev]
+eigen:
+  debian:
+    stretch: [libeigen3-dev]
+geographiclib-tools:
+  debian:
+    stretch: [geographiclib-tools]
+geographiclib:
+  debian:
+    stretch: [libgeographic-dev]
+google-mock:
+  debian:
+    stretch: [google-mock]
+graphviz:
+  debian:
+    stretch: [graphviz]
+gtest:
+  debian:
+    stretch: [libgtest-dev]
+libconsole-bridge-dev:
+  debian:
+    stretch: [libconsole-bridge-dev]
+libjpeg:
+  debian:
+    stretch: [libjpeg-dev]
+libpng-dev:
+  debian:
+    stretch: [libpng-dev]
+libpoco-dev:
+  debian:
+    stretch: [libpoco-dev]
+libssl-dev:
+  debian:
+    stretch: [libssl-dev]
+libtiff-dev:
+  debian:
+    stretch: [libtiff5-dev]
+liburdfdom-dev:
+  debian:
+    stretch: [liburdfdom-dev]
+liburdfdom-headers-dev:
+  debian:
+    stretch: [liburdfdom-headers-dev]
+libv4l-dev:
+  debian:
+    stretch: [libv4l-dev]
+libvtk-qt:
+  debian:
+    stretch: [libvtk6-qt-dev]
+libwebp-dev:
+  debian:
+    stretch: [libwebp-dev]
+log4cxx:
+  debian:
+    stretch: [liblog4cxx-dev]
+lz4:
+  debian:
+    stretch: [liblz4-dev]
+pkg-config:
+  debian:
+    stretch: [pkg-config]
+protobuf:
+  debian:
+    stretch: [libprotobuf10]
+python-bson:
+  debian:
+    stretch: [python-bson]
+python-catkin-pkg:
+  debian:
+    stretch: [python-catkin-pkg]
+python-coverage:
+  debian:
+    stretch: [python-coverage]
+python-defusedxml:
+  debian:
+    stretch: [python-defusedxml]
+python-empy:
+  debian:
+    stretch: [python-empy]
+python-future:
+  debian:
+    stretch: [python-future]
+python-imaging:
+  debian:
+    stretch: [python-imaging]
+python-lxml:
+  debian:
+    stretch: [python-lxml]
+python-mock:
+  debian:
+    stretch: [python-mock]
+python-netifaces:
+  debian:
+    stretch: [python-netifaces]
+python-nose:
+  debian:
+    stretch: [python-nose]
+python-numpy:
+  debian:
+    stretch: [python-numpy]
+python-paramiko:
+  debian:
+    stretch: [python-paramiko]
+python-rosdep:
+  debian:
+    stretch: [python-rosdep]
+python-rospkg:
+  debian:
+    stretch: [python-rospkg]
+python-serial:
+  debian:
+    stretch: [python-serial]
+python-setuptools:
+  debian:
+    stretch: [python-setuptools]
+python-sip:
+  debian:
+    stretch: [python-sip-dev]
+python-tornado:
+  debian:
+    stretch: [python-tornado]
+python-twisted-core:
+  debian:
+    stretch: [python-twisted-core]
+python-websocket:
+  debian:
+    stretch: [python-websocket]
+python-wxtools:
+  debian:
+    stretch: [python-wxtools]
+python-yaml:
+  debian:
+    stretch: [python-yaml]
+python:
+  debian:
+    stretch: [python-dev]
+sbcl:
+  debian:
+    stretch: [sbcl]
+tinyxml2:
+  debian:
+    stretch: [libtinyxml2-dev]
+tinyxml:
+  debian:
+    stretch: [libtinyxml-dev]
+uuid:
+  debian:
+    stretch: [uuid-dev]
+web_video_server:
+  debian:
+    stretch: [ros-kinetic-web-video-server]
+v4l-utils:
+  debian:
+    stretch: [v4l-utils]
+yaml-cpp:
+  debian:
+    stretch: [libyaml-cpp-dev]
+zlib:
+  debian:
+    stretch: [zlib1g-dev]
+compressed_depth_image_transport:
+  debian:
+    stretch: [ros-kinetic-compressed-depth-image-transport]
+compressed_image_transport:
+  debian:
+    stretch: [ros-kinetic-compressed-image-transport]
+dynamic_reconfigure:
+  debian:
+    stretch: [ros-kinetic-dynamic-reconfigure]
+theora_image_transport:
+  debian:
+    stretch: [ros-kinetic-theora-image-transport]
+libogg:
+  debian:
+    stretch: [libtheora0=1.1.1+dfsg.1-14]

builder/assets/monkey-clever

@@ -20,7 +20,7 @@
     # Example:
     #      DocumentRoot /home/krypton/htdocs
 
-    DocumentRoot /home/pi/catkin_ws/src/clever/clever/static
+    DocumentRoot /usr/share/monkey-static
 
     # Redirect:
     # ---------
@@ -36,13 +36,13 @@
     # ----------
     # Registration file of correct request.
 
-    AccessLog /home/pi/monkey/build/log/access.log
+    AccessLog /var/log/monkey-clever/access.log
 
     # ErrorLog:
     # ---------
     # Registration file of incorrect request.
 
-    ErrorLog /home/pi/monkey/build/log/error.log
+    ErrorLog /var/log/monkey-clever/error.log
 
 [ERROR_PAGES]
     404  404.html

builder/assets/monkey.service

@@ -0,0 +1,9 @@
+[Unit]
+Description=Monkey web-server
+
+[Service]
+ExecStart=/usr/sbin/monkey --port 80 --workers 1
+Restart=on-abort
+
+[Install]
+WantedBy=multi-user.target

builder/assets/roscore.service

@@ -3,7 +3,7 @@ Description=Launcher for the ROS master, parameter server and rosout logging nod
 After=network.target
 
 [Service]
-EnvironmentFile=/home/pi/catkin_ws/src/clever/deploy/roscore.env
+EnvironmentFile=/lib/systemd/system/roscore.env
 ExecStart=/opt/ros/kinetic/bin/roscore
 Restart=on-abort
 

builder/image-build.sh

@@ -0,0 +1,106 @@
+#! /usr/bin/env bash
+
+#
+# Script for build the image. Used builder script of the target repo
+# For build: docker run --privileged -it --rm -v /dev:/dev -v $(pwd):/builder/repo smirart/builder
+#
+# Copyright (C) 2018 Copter Express Technologies
+#
+# Author: Artem Smirnov <urpylka@gmail.com>
+#
+
+set -e # Exit immidiately on non-zero result
+
+SOURCE_IMAGE="http://repo.coex.space/2018-06-27-raspbian-stretch-lite.zip"
+
+export DEBIAN_FRONTEND=${DEBIAN_FRONTEND:='noninteractive'}
+export LANG=${LANG:='C.UTF-8'}
+export LC_ALL=${LC_ALL:='C.UTF-8'}
+
+echo_stamp() {
+  # TEMPLATE: echo_stamp <TEXT> <TYPE>
+  # TYPE: SUCCESS, ERROR, INFO
+
+  # More info there https://www.shellhacks.com/ru/bash-colors/
+
+  TEXT="$(date '+[%Y-%m-%d %H:%M:%S]') $1"
+  TEXT="\e[1m$TEXT\e[0m" # BOLD
+
+  case "$2" in
+    SUCCESS)
+    TEXT="\e[32m${TEXT}\e[0m";; # GREEN
+    ERROR)
+    TEXT="\e[31m${TEXT}\e[0m";; # RED
+    *)
+    TEXT="\e[34m${TEXT}\e[0m";; # BLUE
+  esac
+  echo -e ${TEXT}
+}
+
+BUILDER_DIR="/builder"
+REPO_DIR="${BUILDER_DIR}/repo"
+SCRIPTS_DIR="${REPO_DIR}/builder"
+IMAGES_DIR="${REPO_DIR}/images"
+
+[[ ! -d ${SCRIPTS_DIR} ]] && (echo_stamp "Directory ${SCRIPTS_DIR} doesn't exist" "ERROR"; exit 1)
+[[ ! -d ${IMAGES_DIR} ]] && mkdir ${IMAGES_DIR} && echo_stamp "Directory ${IMAGES_DIR} was created successful" "SUCCESS"
+
+if [[ -z ${TRAVIS_TAG} ]]; then IMAGE_VERSION="$(cd ${REPO_DIR}; git log --format=%h -1)"; else IMAGE_VERSION="${TRAVIS_TAG}"; fi
+# IMAGE_VERSION="${TRAVIS_TAG:=$(cd ${REPO_DIR}; git log --format=%h -1)}"
+REPO_URL="$(cd ${REPO_DIR}; git remote --verbose | grep origin | grep fetch | cut -f2 | cut -d' ' -f1 | sed 's/git@github\.com\:/https\:\/\/github.com\//')"
+REPO_NAME="$(basename -s '.git' ${REPO_URL})"
+IMAGE_NAME="${REPO_NAME}_${IMAGE_VERSION}.img"
+IMAGE_PATH="${IMAGES_DIR}/${IMAGE_NAME}"
+
+get_image() {
+  # TEMPLATE: get_image <IMAGE_PATH> <RPI_DONWLOAD_URL> 
+  local BUILD_DIR=$(dirname $1)
+  local RPI_ZIP_NAME=$(basename $2)
+  local RPI_IMAGE_NAME=$(echo ${RPI_ZIP_NAME} | sed 's/zip/img/')
+
+  if [ ! -e "${BUILD_DIR}/${RPI_ZIP_NAME}" ]; then
+    echo_stamp "Downloading original Linux distribution" \
+    && wget -nv -O ${BUILD_DIR}/${RPI_ZIP_NAME} $2 \
+    && echo_stamp "Downloading complete" "SUCCESS" \
+    || (echo_stamp "Downloading was failed!" "ERROR"; exit 1)
+  else echo_stamp "Linux distribution already donwloaded"; fi
+
+  echo_stamp "Unzipping Linux distribution image" \
+  && unzip -p ${BUILD_DIR}/${RPI_ZIP_NAME} ${RPI_IMAGE_NAME} > $1 \
+  && echo_stamp "Unzipping complete" "SUCCESS" \
+  || (echo_stamp "Unzipping was failed!" "ERROR"; exit 1)
+}
+
+get_image ${IMAGE_PATH} ${SOURCE_IMAGE}
+
+# Make free space
+${BUILDER_DIR}/image-resize.sh ${IMAGE_PATH} max '7G'
+
+${BUILDER_DIR}/image-chroot.sh ${IMAGE_PATH} copy ${SCRIPTS_DIR}'/assets/init_rpi.sh' '/root/'
+${BUILDER_DIR}/image-chroot.sh ${IMAGE_PATH} copy ${SCRIPTS_DIR}'/assets/hardware_setup.sh' '/root/'
+${BUILDER_DIR}/image-chroot.sh ${IMAGE_PATH} exec ${SCRIPTS_DIR}'/image-init.sh' ${IMAGE_VERSION} ${SOURCE_IMAGE}
+
+# Monkey
+${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/'
+
+# 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_DIR}/image-chroot.sh ${IMAGE_PATH} copy ${SCRIPTS_DIR}'/assets/monkey.service' '/lib/systemd/system/'
+# software install
+${BUILDER_DIR}/image-chroot.sh ${IMAGE_PATH} exec ${SCRIPTS_DIR}'/image-software.sh'
+# network setup
+${BUILDER_DIR}/image-chroot.sh ${IMAGE_PATH} exec ${SCRIPTS_DIR}'/image-network.sh'
+
+# If RPi then use a one thread to build a ROS package on RPi, else use all
+[[ $(arch) == 'armv7l' ]] && NUMBER_THREADS=1 || NUMBER_THREADS=$(nproc --all)
+# Clever
+${BUILDER_DIR}/image-chroot.sh ${IMAGE_PATH} copy ${SCRIPTS_DIR}'/assets/clever.service' '/lib/systemd/system/'
+${BUILDER_DIR}/image-chroot.sh ${IMAGE_PATH} copy ${SCRIPTS_DIR}'/assets/roscore.env' '/lib/systemd/system/'
+${BUILDER_DIR}/image-chroot.sh ${IMAGE_PATH} copy ${SCRIPTS_DIR}'/assets/roscore.service' '/lib/systemd/system/'
+${BUILDER_DIR}/image-chroot.sh ${IMAGE_PATH} copy ${SCRIPTS_DIR}'/assets/kinetic-rosdep-clever.yaml' '/etc/ros/rosdep/'
+# ${BUILDER_DIR}/image-chroot.sh ${IMAGE_PATH} copy ${SCRIPTS_DIR}'/assets/kinetic-ros-clever.rosinstall' '/home/pi/ros_catkin_ws/'
+${BUILDER_DIR}/image-chroot.sh ${IMAGE_PATH} exec ${SCRIPTS_DIR}'/image-ros.sh' ${REPO_URL} ${IMAGE_VERSION} false false ${NUMBER_THREADS} 
+
+${BUILDER_DIR}/image-resize.sh ${IMAGE_PATH}

builder/image-init.sh

@@ -0,0 +1,54 @@
+#! /usr/bin/env bash
+
+#
+# Script for initialisation image
+#
+# Copyright (C) 2018 Copter Express Technologies
+#
+# Author: Artem Smirnov <urpylka@gmail.com>
+#
+
+set -e # Exit immidiately on non-zero result
+
+echo_stamp() {
+  # TEMPLATE: echo_stamp <TEXT> <TYPE>
+  # TYPE: SUCCESS, ERROR, INFO
+
+  # More info there https://www.shellhacks.com/ru/bash-colors/
+
+  TEXT="$(date '+[%Y-%m-%d %H:%M:%S]') $1"
+  TEXT="\e[1m$TEXT\e[0m" # BOLD
+
+  case "$2" in
+    SUCCESS)
+    TEXT="\e[32m${TEXT}\e[0m";; # GREEN
+    ERROR)
+    TEXT="\e[31m${TEXT}\e[0m";; # RED
+    *)
+    TEXT="\e[34m${TEXT}\e[0m";; # BLUE
+  esac
+  echo -e ${TEXT}
+}
+
+echo_stamp "Write CLEVER information"
+
+# Clever image version
+echo "$1" >> /etc/clever_version
+# Origin image file name
+echo "${2%.*}" >> /etc/clever_origin
+
+echo_stamp "Write magic script to /etc/rc.local"
+MAGIC_SCRIPT="sudo /root/init_rpi.sh; sudo sed -i '/sudo \\\/root\\\/init_rpi.sh/d' /etc/rc.local && sudo reboot"
+sed -i "19a${MAGIC_SCRIPT}" /etc/rc.local
+
+# It needs for autosizer.sh & maybe that is correct
+echo_stamp "Change boot partition"
+sed -i 's/root=[^ ]*/root=\/dev\/mmcblk0p2/' /boot/cmdline.txt
+sed -i 's/.*  \/boot           vfat    defaults          0       2$/\/dev\/mmcblk0p1  \/boot           vfat    defaults          0       2/' /etc/fstab
+sed -i 's/.*  \/               ext4    defaults,noatime  0       1$/\/dev\/mmcblk0p2  \/               ext4    defaults,noatime  0       1/' /etc/fstab
+
+echo_stamp "Set max space for syslogs"
+# https://unix.stackexchange.com/questions/139513/how-to-clear-journalctl
+sed -i 's/#SystemMaxUse=/SystemMaxUse=200M/' /etc/systemd/journald.conf
+
+echo_stamp "End of init image"

builder/image-network.sh

@@ -0,0 +1,73 @@
+#! /usr/bin/env bash
+
+#
+# Script for network configure
+#
+# Copyright (C) 2018 Copter Express Technologies
+#
+# Author: Artem Smirnov <urpylka@gmail.com>
+#
+
+set -e # Exit immidiately on non-zero result
+
+echo_stamp() {
+  # TEMPLATE: echo_stamp <TEXT> <TYPE>
+  # TYPE: SUCCESS, ERROR, INFO
+
+  # More info there https://www.shellhacks.com/ru/bash-colors/
+
+  TEXT="$(date '+[%Y-%m-%d %H:%M:%S]') $1"
+  TEXT="\e[1m$TEXT\e[0m" # BOLD
+
+  case "$2" in
+    SUCCESS)
+    TEXT="\e[32m${TEXT}\e[0m";; # GREEN
+    ERROR)
+    TEXT="\e[31m${TEXT}\e[0m";; # RED
+    *)
+    TEXT="\e[34m${TEXT}\e[0m";; # BLUE
+  esac
+  echo -e ${TEXT}
+}
+
+echo_stamp "#1 Write to /etc/wpa_supplicant/wpa_supplicant.conf"
+
+# TODO: Use wpa_cli insted direct file edit
+cat << EOF >> /etc/wpa_supplicant/wpa_supplicant.conf
+network={
+    ssid="CLEVER"
+    psk="cleverwifi"
+    mode=2
+    proto=RSN
+    key_mgmt=WPA-PSK
+    pairwise=CCMP
+    group=CCMP
+    auth_alg=OPEN
+}
+EOF
+
+echo_stamp "#2 Write STATIC to /etc/dhcpcd.conf"
+
+cat << EOF >> /etc/dhcpcd.conf
+interface wlan0
+static ip_address=192.168.11.1/24
+EOF
+
+echo_stamp "#3 Write dhcp-config to /etc/dnsmasq.conf"
+
+cat << EOF >> /etc/dnsmasq.conf
+interface=wlan0
+address=/clever/coex/192.168.11.1
+dhcp-range=192.168.11.100,192.168.11.200,12h
+no-hosts
+filterwin2k
+bogus-priv
+domain-needed
+quiet-dhcp6
+EOF
+
+#echo_stamp "#4 Write magic script for rename SSID to /etc/rc.local"
+#RENAME_SSID="sudo sed -i.OLD \"s/CLEVER/CLEVER-\$(head -c 100 /dev/urandom | xxd -ps -c 100 | sed -e 's/[^0-9]//g' | cut -c 1-4)/g\" /etc/wpa_supplicant/wpa_supplicant.conf && sudo sed -i '/sudo sed/d' /etc/rc.local && sudo reboot"
+#sed -i "19a$RENAME_SSID" /etc/rc.local
+
+echo_stamp "#5 End of network installation"

builder/image-ros.sh

@@ -0,0 +1,176 @@
+#! /usr/bin/env bash
+
+#
+# Script for build the image. Used builder script of the target repo
+# For build: docker run --privileged -it --rm -v /dev:/dev -v $(pwd):/builder/repo smirart/builder
+#
+# Copyright (C) 2018 Copter Express Technologies
+#
+# Author: Artem Smirnov <urpylka@gmail.com>
+#
+
+set -e # Exit immidiately on non-zero result
+
+REPO=$1
+REF=$2
+INSTALL_ROS_PACK_SOURCES=$3
+DISCOVER_ROS_PACK=$4
+NUMBER_THREADS=$5
+
+echo_stamp() {
+  # TEMPLATE: echo_stamp <TEXT> <TYPE>
+  # TYPE: SUCCESS, ERROR, INFO
+
+  # More info there https://www.shellhacks.com/ru/bash-colors/
+
+  TEXT="$(date '+[%Y-%m-%d %H:%M:%S]') $1"
+  TEXT="\e[1m$TEXT\e[0m" # BOLD
+
+  case "$2" in
+    SUCCESS)
+    TEXT="\e[32m${TEXT}\e[0m";; # GREEN
+    ERROR)
+    TEXT="\e[31m${TEXT}\e[0m";; # RED
+    *)
+    TEXT="\e[34m${TEXT}\e[0m";; # BLUE
+  esac
+  echo -e ${TEXT}
+}
+
+# https://gist.github.com/letmaik/caa0f6cc4375cbfcc1ff26bd4530c2a3
+# https://github.com/travis-ci/travis-build/blob/master/lib/travis/build/templates/header.sh
+my_travis_retry() {
+  local result=0
+  local count=1
+  while [ $count -le 3 ]; do
+    [ $result -ne 0 ] && {
+      echo -e "\n${ANSI_RED}The command \"$@\" failed. Retrying, $count of 3.${ANSI_RESET}\n" >&2
+    }
+    # ! { } ignores set -e, see https://stackoverflow.com/a/4073372
+    ! { "$@"; result=$?; }
+    [ $result -eq 0 ] && break
+    count=$(($count + 1))
+    sleep 1
+  done
+
+  [ $count -gt 3 ] && {
+    echo -e "\n${ANSI_RED}The command \"$@\" failed 3 times.${ANSI_RESET}\n" >&2
+  }
+
+  return $result
+}
+
+# TODO: 'kinetic-rosdep-clever.yaml' should add only if we use our repo?
+echo_stamp "Init rosdep" \
+&& rosdep init \
+&& echo "yaml file:///etc/ros/rosdep/kinetic-rosdep-clever.yaml" >> /etc/ros/rosdep/sources.list.d/20-default.list \
+&& rosdep update
+
+resolve_rosdep() {
+  # TEMPLATE: resolve_rosdep <CATKIN_PATH> <ROS_DISTRO> <OS_DISTRO> <OS_VERSION>
+  CATKIN_PATH=$1
+  ROS_DISTRO='kinetic'
+  OS_DISTRO='debian'
+  OS_VERSION='stretch'
+
+  echo_stamp "Installing dependencies apps with rosdep in ${CATKIN_PATH}"
+  cd ${CATKIN_PATH}
+  my_travis_retry rosdep install -y --from-paths src --ignore-src --rosdistro ${ROS_DISTRO} -r --os=${OS_DISTRO}:${OS_VERSION}
+}
+
+INSTALL_ROS_PACK_SOURCES=${INSTALL_ROS_PACK_SOURCES:='false'}
+if [ "${INSTALL_ROS_PACK_SOURCES}" = "true" ]; then
+  DISCOVER_ROS_PACK=${DISCOVER_ROS_PACK:='true'}
+  if [ "${DISCOVER_ROS_PACK}" = "false" ]; then
+    echo_stamp "Preparing ros_comm packages to kinetic-ros_comm-wet.rosinstall" \
+    && mkdir -p /home/pi/ros_catkin_ws && cd /home/pi/ros_catkin_ws \
+    && rosinstall_generator ros_comm --rosdistro kinetic --deps --wet-only --tar > kinetic-ros_comm-wet.rosinstall \
+    && wstool init -j${NUMBER_THREADS} src kinetic-ros_comm-wet.rosinstall \
+    && echo_stamp "All roscomm sources was installed!" "SUCCESS" \
+    || (echo_stamp "Some roscomm sources installation was failed!" "ERROR"; exit 1)
+
+    echo_stamp "Preparing other ROS-packages to kinetic-custom_ros.rosinstall" \
+    && cd /home/pi/ros_catkin_ws \
+    && rosinstall_generator \
+    actionlib actionlib_msgs angles async_web_server_cpp bond bond_core bondcpp bondpy camera_calibration_parsers camera_info_manager catkin class_loader cmake_modules cpp_common cv_bridge cv_camera diagnostic_msgs diagnostic_updater dynamic_reconfigure eigen_conversions gencpp geneus genlisp genmsg gennodejs genpy geographic_msgs geometry_msgs geometry2 image_transport compressed_image_transport libmavconn mavlink mavros_msgs message_filters message_generation message_runtime mk nav_msgs nodelet orocos_kdl pluginlib python_orocos_kdl ros ros_comm rosapi rosauth rosbag rosbag_migration_rule rosbag_storage rosbash rosboost_cfg rosbridge_library rosbridge_server rosbridge_suite rosbuild rosclean rosconsole rosconsole_bridge roscpp roscpp_serialization roscpp_traits roscreate rosgraph rosgraph_msgs roslang roslaunch roslib roslint roslisp roslz4 rosmake rosmaster rosmsg rosnode rosout rospack rosparam rospy rospy_tutorials rosserial rosserial_client rosserial_msgs rosserial_python rosservice rostest rostime rostopic rosunit roswtf sensor_msgs smclib std_msgs std_srvs stereo_msgs tf tf2 tf2_bullet tf2_eigen tf2_geometry_msgs tf2_kdl tf2_msgs tf2_py tf2_ros tf2_sensor_msgs tf2_tools topic_tools trajectory_msgs urdf urdf_parser_plugin usb_cam uuid_msgs visualization_msgs web_video_server xmlrpcpp mavros opencv3 mavros_extras interactive_markers tf2_web_republisher interactive_marker_proxy \
+    --rosdistro kinetic --deps --wet-only --tar > kinetic-custom_ros.rosinstall \
+    && wstool merge -j${NUMBER_THREADS} -t src kinetic-custom_ros.rosinstall \
+    && wstool update -j${NUMBER_THREADS} -t src \
+    && echo_stamp "All custom sources was installed!" "SUCCESS" \
+    || (echo_stamp "Some custom sources installation was failed!" "ERROR"; exit 1)
+  else
+    echo_stamp "Creating ros_catkin_ws & getting all sources using wstool" \
+    && mkdir -p /home/pi/ros_catkin_ws && cd /home/pi/ros_catkin_ws \
+    && wstool init -j${NUMBER_THREADS} src kinetic-ros-clever.rosinstall \
+    > /dev/null \
+    && echo_stamp "All CLEVER sources was installed!" "SUCCESS" \
+    || (echo_stamp "Some CLEVER sources installation was failed!" "ERROR"; exit 1)
+  fi
+
+  resolve_rosdep '/home/pi/ros_catkin_ws'
+
+  # TODO: Add refactor to origin repo
+  #echo_stamp "Refactoring usb_cam in SRC"
+  #sed -i '/#define __STDC_CONSTANT_MACROS/a\#define PIX_FMT_RGB24 AV_PIX_FMT_RGB24\n#define PIX_FMT_YUV422P AV_PIX_FMT_YUV422P' /home/pi/ros_catkin_ws/src/usb_cam/src/usb_cam.cpp
+
+  echo_stamp "Building ros_catkin_ws packages"
+  cd /home/pi/ros_catkin_ws && ./src/catkin/bin/catkin_make_isolated --install -j${NUMBER_THREADS} -DCMAKE_BUILD_TYPE=Release --install-space /opt/ros/kinetic
+
+  #echo_stamp "#11 Building light packages on 2 threads"
+  #cd /home/pi/ros_catkin_ws && ./src/catkin/bin/catkin_make_isolated --install -DCMAKE_BUILD_TYPE=Release -j2 --install-space /opt/ros/kinetic --pkg actionlib actionlib_msgs angles async_web_server_cpp bond bond_core bondcpp bondpy camera_calibration_parsers camera_info_manager catkin class_loader cmake_modules cpp_common diagnostic_msgs diagnostic_updater dynamic_reconfigure eigen_conversions gencpp geneus genlisp genmsg gennodejs genpy geographic_msgs geometry_msgs geometry2 image_transport libmavconn mavlink mavros_msgs message_filters message_generation message_runtime mk nav_msgs nodelet orocos_kdl pluginlib python_orocos_kdl ros ros_comm rosapi rosauth rosbag rosbag_migration_rule rosbag_storage rosbash rosboost_cfg rosbridge_library rosbridge_server rosbridge_suite rosbuild rosclean rosconsole rosconsole_bridge roscpp roscpp_serialization roscpp_traits roscreate rosgraph rosgraph_msgs roslang roslaunch roslib roslint roslisp roslz4 rosmake rosmaster rosmsg rosnode rosout rospack rosparam rospy rospy_tutorials rosserial rosserial_client rosserial_msgs rosserial_python rosservice rostest rostime rostopic rosunit roswtf sensor_msgs smclib std_msgs std_srvs stereo_msgs tf tf2 tf2_bullet tf2_eigen tf2_geometry_msgs tf2_kdl tf2_msgs tf2_py tf2_ros tf2_sensor_msgs tf2_tools topic_tools trajectory_msgs urdf urdf_parser_plugin usb_cam uuid_msgs visualization_msgs xmlrpcpp
+
+  #echo_stamp "#12 Building heavy packages"
+  # This command uses less threads to avoid Raspberry Pi freeze
+  #cd /home/pi/ros_catkin_ws && ./src/catkin/bin/catkin_make_isolated --install -DCMAKE_BUILD_TYPE=Release -j1 --install-space /opt/ros/kinetic --pkg mavros opencv3 cv_bridge cv_camera mavros_extras web_video_server
+
+  # Install builded packages
+  # WARNING: A major bug was found when using --pkg option (catkin_make_isolated doesn't install environment files)
+  # TODO: Can we increase threads number with HDD swap?
+
+  echo_stamp "Remove build_isolated & devel_isolated from ros_catkin_ws"
+  rm -rf /home/pi/ros_catkin_ws/build_isolated /home/pi/ros_catkin_ws/devel_isolated
+  chown -Rf pi:pi /home/pi/ros_catkin_ws
+fi
+
+echo_stamp "Installing CLEVER" \
+&& git clone ${REPO} /home/pi/catkin_ws/src/clever \
+&& cd /home/pi/catkin_ws/src/clever \
+&& echo "REF: ${REF}" \
+&& git checkout ${REF} \
+&& cd /home/pi/catkin_ws \
+&& resolve_rosdep $(pwd) \
+&& my_travis_retry pip install wheel \
+&& my_travis_retry pip install -r /home/pi/catkin_ws/src/clever/clever/requirements.txt \
+&& source /opt/ros/kinetic/setup.bash \
+&& catkin_make -j${NUMBER_THREADS} -DCMAKE_BUILD_TYPE=Release \
+&& systemctl enable roscore \
+&& systemctl enable clever \
+&& echo_stamp "All CLEVER was installed!" "SUCCESS" \
+|| (echo_stamp "CLEVER installation was failed!" "ERROR"; exit 1)
+
+# TODO move GeographicLib datasets to Mavros debian package
+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 "Change permissions for catkin_ws"
+chown -Rf pi:pi /home/pi/catkin_ws
+
+echo_stamp "Setup ROS environment"
+cat << EOF >> /home/pi/.bashrc
+LANG='C.UTF-8'
+LC_ALL='C.UTF-8'
+ROS_DISTRO='kinetic'
+export ROS_IP='192.168.11.1'
+source /opt/ros/kinetic/setup.bash
+source /home/pi/catkin_ws/devel/setup.bash
+EOF
+
+#echo_stamp "Removing local apt mirror"
+# Restore original sources.list
+#mv /var/sources.list.bak /etc/apt/sources.list
+# Clean apt cache
+apt-get clean -qq > /dev/null
+# Remove local mirror repository key
+#apt-key del COEX-MIRROR
+
+echo_stamp "END of ROS INSTALLATION"

builder/image-software.sh

@@ -0,0 +1,130 @@
+#! /usr/bin/env bash
+
+#
+# Script for install software to the image.
+#
+# Copyright (C) 2018 Copter Express Technologies
+#
+# Author: Artem Smirnov <urpylka@gmail.com>
+#
+
+set -e # Exit immidiately on non-zero result
+
+echo_stamp() {
+  # TEMPLATE: echo_stamp <TEXT> <TYPE>
+  # TYPE: SUCCESS, ERROR, INFO
+
+  # More info there https://www.shellhacks.com/ru/bash-colors/
+
+  TEXT="$(date '+[%Y-%m-%d %H:%M:%S]') $1"
+  TEXT="\e[1m${TEXT}\e[0m" # BOLD
+
+  case "$2" in
+    SUCCESS)
+    TEXT="\e[32m${TEXT}\e[0m";; # GREEN
+    ERROR)
+    TEXT="\e[31m${TEXT}\e[0m";; # RED
+    *)
+    TEXT="\e[34m${TEXT}\e[0m";; # BLUE
+  esac
+  echo -e ${TEXT}
+}
+
+# https://gist.github.com/letmaik/caa0f6cc4375cbfcc1ff26bd4530c2a3
+# https://github.com/travis-ci/travis-build/blob/master/lib/travis/build/templates/header.sh
+my_travis_retry() {
+  local result=0
+  local count=1
+  while [ $count -le 3 ]; do
+    [ $result -ne 0 ] && {
+      echo -e "\n${ANSI_RED}The command \"$@\" failed. Retrying, $count of 3.${ANSI_RESET}\n" >&2
+    }
+    # ! { } ignores set -e, see https://stackoverflow.com/a/4073372
+    ! { "$@"; result=$?; }
+    [ $result -eq 0 ] && break
+    count=$(($count + 1))
+    sleep 1
+  done
+
+  [ $count -gt 3 ] && {
+    echo -e "\n${ANSI_RED}The command \"$@\" failed 3 times.${ANSI_RESET}\n" >&2
+  }
+
+  return $result
+}
+
+echo_stamp "Install apt keys & repos"
+
+# TODO: This STDOUT consist 'OK'
+curl http://repo.coex.space/aptly_repo_signing.key 2> /dev/null | apt-key add -
+apt-get update \
+&& apt-get install --no-install-recommends -y -qq dirmngr=2.1.18-8~deb9u2 > /dev/null \
+&& apt-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
+echo "deb http://repo.coex.space/rpi-ros-kinetic stretch main" > /etc/apt/sources.list.d/rpi-ros-kinetic.list
+echo "deb http://repo.coex.space/clever stretch main" > /etc/apt/sources.list.d/clever.list
+
+echo_stamp "Update apt cache"
+
+# TODO: FIX ERROR: /usr/bin/apt-key: 596: /usr/bin/apt-key: cannot create /dev/null: Permission denied
+apt-get update -qq
+# && apt upgrade -y
+
+echo_stamp "Software installing"
+apt-get install --no-install-recommends -y \
+unzip=6.0-21 \
+zip=3.0-11 \
+ipython=5.1.0-3 \
+ipython3=5.1.0-3 \
+screen=4.5.0-6 \
+byobu=5.112-1  \
+nmap=7.40-1 \
+lsof=4.89+dfsg-0.1 \
+git \
+dnsmasq=2.76-5+rpt1+deb9u1  \
+tmux=2.3-4 \
+vim=2:8.0.0197-4+deb9u1 \
+cmake=3.7.2-1 \
+python-pip=9.0.1-2+rpt2 \
+python3-pip=9.0.1-2+rpt2 \
+libjpeg8-dev=8d1-2 \
+tcpdump \
+ltrace \
+libpoco-dev=1.7.6+dfsg1-5+deb9u1 \
+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 \
+&& echo_stamp "Everything was installed!" "SUCCESS" \
+|| (echo_stamp "Some packages wasn't installed!" "ERROR"; exit 1)
+
+# Deny byobu to check available updates
+sed -i "s/updates_available//" /usr/share/byobu/status/status
+# sed -i "s/updates_available//" /home/pi/.byobu/status
+
+echo_stamp "Upgrade pip"
+my_travis_retry pip install --upgrade pip
+my_travis_retry pip3 install --upgrade pip3
+
+echo_stamp "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 "Setup Monkey"
+mv /etc/monkey/sites/default /etc/monkey/sites/default.orig
+mv /root/monkey-clever /etc/monkey/sites/default
+systemctl enable monkey.service
+
+echo_stamp "Add .vimrc"
+cat << EOF > /home/pi/.vimrc
+set mouse-=a
+syntax on
+autocmd BufNewFile,BufRead *.launch set syntax=xml
+EOF
+
+echo_stamp "End of software installation"

clever/CMakeLists.txt

@@ -165,10 +165,18 @@ add_executable(rc src/rc.cpp)
 
 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"/>
-    <remap from="camera_info" to="main_camera/camera_info"/>
+    <arg name="aruco_detect" default="true"/>
+    <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">
-        <param name="frame_id" value="aruco_map_raw"/>
-        <param name="type" value="gridboard"/>
-        <param name="markers_x" value="1"/>
-        <param name="markers_y" value="6"/>
-        <param name="first_marker" value="240"/>
-        <param name="markers_side" value="0.3362"/>
-        <param name="markers_sep" value="0.46"/>
+    <!-- detect aruco markers, estimate poses -->
+    <node pkg="nodelet" if="$(arg aruco_detect)" clear_params="true" type="nodelet" name="aruco_detect" args="load aruco_pose/aruco_detect nodelet_manager" output="screen">
+        <remap from="image_raw" to="main_camera/image_raw"/>
+        <remap from="camera_info" to="main_camera/camera_info"/>
 
-        <!-- Custom gridboard: -->
-        <!--<rosparam param="marker_ids">[6, 5, 4, 3, 2, 1]</rosparam>-->
+        <param name="length" value="0.32"/>
+        <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">
-        <param name="aruco_orientation" value="local_origin"/>
-        <!--<param name="aruco_orientation" value="local_origin_upside_down"/>-->
+    <!-- estimate aruco map pose -->
+    <node pkg="nodelet" clear_params="true" type="nodelet" name="aruco_map" if="$(arg aruco_map)" args="load aruco_pose/aruco_map nodelet_manager" output="screen">
+        <remap from="markers" to="aruco_detect/markers"/>
+        <remap from="camera_info" to="main_camera/camera_info"/>
+        <param name="type" value="map"/>
+        <param name="map" value="$(find aruco_pose)/map/map.txt"/>
+        <param name="snap_orientation" value="local_origin"/>
+    </node>
 
-        <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

@@ -2,14 +2,14 @@
     <arg name="fcu_conn" default="usb"/>
     <arg name="fcu_ip" default="127.0.0.1"/>
     <arg name="gcs_bridge" default="tcp"/>
-    <arg name="web_server" default="true"/>
     <arg name="web_video_server" default="true"/>
     <arg name="rosbridge" default="true"/>
     <arg name="main_camera" default="true"/>
     <arg name="optical_flow" default="false"/>
     <arg name="aruco" default="false"/>
-    <arg name="rc" value="true"/>
+    <arg name="rc" default="true"/>
     <arg name="arduino" default="false"/>
+    <arg name="vl53l1x" default="false"/>
 
     <!-- mavros -->
     <include file="$(find clever)/launch/mavros.launch">
@@ -19,14 +19,10 @@
         <arg name="viz" value="true"/>
     </include>
 
-
-    <!-- web server, serving /home/pi/catkin_ws/src/clever/clever/static -->
-    <node name="web_server" pkg="clever" type="monkey" output="screen" if="$(arg web_server)" respawn="true" respawn_delay="5"/>
-
     <!-- 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 -->
@@ -37,16 +33,19 @@
 
     <!-- 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"/>
 
-    <!-- fcu_horiz frame -->
-    <node pkg="nodelet" type="nodelet" name="fcu_horiz" args="standalone clever/fcu_horiz" output="screen" clear_params="true"/>
 
     <!-- simplified offboard control -->
     <node name="simple_offboard" pkg="clever" type="simple_offboard.py" output="screen"/>
+    <!-- Auxiliary frames -->
+    <node name="frames" pkg="clever" type="frames" output="screen">
+        <param name="body/frame_id" value="fcu_horiz"/>
+    </node>
+
 
     <!-- main camera -->
     <include file="$(find clever)/launch/main_camera.launch" if="$(arg main_camera)"/>
@@ -54,6 +53,12 @@
     <!-- rosbridge -->
     <include file="$(find rosbridge_server)/launch/rosbridge_websocket.launch" if="$(eval rosbridge or rc)"/>
 
+    <!-- vl53l1x ToF rangefinder -->
+    <node name="vl53l1x" pkg="clever" type="vl53l1x.py" output="screen" if="$(arg vl53l1x)">
+        <param name="z_shift" value="-0.05"/>
+        <!-- <remap from="~range" to="mavros/distance_sensor/rangefinder_3_sub"/> -->
+    </node>
+
     <!-- rc backend -->
     <node name="rc" pkg="clever" type="rc" output="screen" if="$(arg rc)"/>
 

clever/launch/main_camera.launch

@@ -19,6 +19,7 @@
         <!-- setting camera FPS -->
         <param name="rate" value="100"/>
         <param name="cv_cap_prop_fps" value="40"/>
+        <param name="capture_delay" value="0.02"/>
 
         <param name="image_width" value="320"/>
         <param name="image_height" value="240"/>

clever/launch/mavros.launch

@@ -76,6 +76,7 @@
             - setpoint_accel
             - trajectory
             - wind_estimation
+            - home_position
         </rosparam>
     </node>
 

clever/launch/sitl.launch

@@ -1,7 +1,7 @@
 <launch>
     <!-- Clever configuration for testing in sitl -->
     <arg name="ip" default="127.0.0.1"/>
-    <arg name="rosbridge" default="true"/>
+    <arg name="rosbridge" default="false"/>
 
     <include file="$(find clever)/launch/clever.launch">
         <arg name="fcu_conn" value="udp"/>
@@ -13,5 +13,7 @@
         <arg name="main_camera" default="false"/>
         <arg name="rosbridge" value="$(arg rosbridge)"/>
         <arg name="aruco" default="false"/>
+        <arg name="vl53l1x" default="false"/>
+        <arg name="rc" default="false"/>
     </include>
 </launch>

clever/package.xml

@@ -2,55 +2,40 @@
 <package>
   <name>clever</name>
   <version>0.0.1</version>
-  <description>The clever package</description>
+  <description>The CLEVER package</description>
 
-  <!-- One maintainer tag required, multiple allowed, one person per tag -->
-  <!-- Example:  -->
-  <!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
   <maintainer email="okalachev@gmail.com">Oleg Kalachev</maintainer>
+  <license>MIT</license>
 
+  <!--url type="website">http://wiki.ros.org/clever</url-->
+  <author email="okalachev@gmail.com">Oleg Kalachev</author>
+  <author email="urpylka@gmail.com">Artem Smirnov</author>
 
-  <!-- One license tag required, multiple allowed, one license per tag -->
-  <!-- Commonly used license strings: -->
-  <!--   BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->
-  <license>TODO</license>
-
-
-  <!-- Url tags are optional, but multiple are allowed, one per tag -->
-  <!-- Optional attribute type can be: website, bugtracker, or repository -->
-  <!-- Example: -->
-  <!-- <url type="website">http://wiki.ros.org/clever</url> -->
-
-
-  <!-- Author tags are optional, multiple are allowed, one per tag -->
-  <!-- Authors do not have to be maintainers, but could be -->
-  <!-- Example: -->
-  <!-- <author email="jane.doe@example.com">Jane Doe</author> -->
-
-
-  <!-- The *_depend tags are used to specify dependencies -->
-  <!-- Dependencies can be catkin packages or system dependencies -->
-  <!-- Examples: -->
   <!-- Use build_depend for packages you need at compile time: -->
-  <!--   <build_depend>message_generation</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>message_runtime</run_depend> -->
-  <!-- Use test_depend for packages you need only for testing: -->
-  <!--   <test_depend>gtest</test_depend> -->
-  <buildtool_depend>catkin</buildtool_depend>
-
   <build_depend>nodelet</build_depend>
   <build_depend>roscpp</build_depend>
-
-  <run_depend>nodelet</run_depend>
+  <build_depend>visualization_msgs</build_depend>
+  <build_depend>tf2_geometry_msgs</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>catkin</run_depend>
   <run_depend>roscpp</run_depend>
+  <run_depend>nodelet</run_depend>
+  <run_depend>mavros</run_depend>
+  <run_depend>mavros_extras</run_depend>
+  <run_depend>lxml</run_depend>
+  <run_depend>cv_camera</run_depend>
+  <run_depend>mjpg-streamer</run_depend>
+  <run_depend>rosbridge_server</run_depend>
+  <run_depend>web_video_server</run_depend>
+  <run_depend>ros_comm</run_depend>r
+  <!-- Use test_depend for packages you need only for testing: -->
+  <!--   <test_depend>gtest</test_depend> -->
 
   <!-- The export tag contains other, unspecified, tags -->
   <export>
-      <nodelet plugin="${prefix}/nodelet_plugins.xml" />
+    <nodelet plugin="${prefix}/nodelet_plugins.xml" />
     <!-- Other tools can request additional information be placed here -->
-
   </export>
 </package>

clever/requirements.txt

@@ -1,3 +1,5 @@
-flask==0.12.2
+flask==0.12.3
 geopy==1.11.0
 pymavlink==2.2.10
+smbus2==0.2.1
+VL53L1X==0.0.2

clever/src/fpv_camera

@@ -4,5 +4,4 @@
 # fpv_camera <video_device> <http port>
 
 echo "Starting FPV camera $1 on :$2"
-cd /home/pi/mjpg-streamer/mjpg-streamer-experimental
-./mjpg_streamer -i "./input_uvc.so -d $1 -r 320x240 -f 30" -o "./output_http.so -w ./www -p $2"
+mjpg_streamer -i "/usr/lib/input_uvc.so -d $1 -r 320x240 -f 30" -o "/usr/lib/output_http.so -w /usr/share/mjpg_streamer/www -p $2"

clever/src/frames.cpp

@@ -0,0 +1,63 @@
+/*
+ * Auxiliary TF frames for CLEVER drone kit:
+ * - Body frame (drone body with zero pitch and roll).
+ * - TODO: REP-0105 `odom` frame emulation: continuous frame without discrete jumps.
+ * - TODO: Terrain frame (base on ALTITUDE message).
+ * - TODO: map_upside_down frame
+ * - TODO: home frame?
+ *
+ * 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.
+ */
+
+// TODO: consider implementing as a mavros plugin
+
+#include <string>
+#include <memory>
+#include <ros/ros.h>
+#include <tf/transform_datatypes.h>
+#include <tf2_ros/transform_broadcaster.h>
+#include <geometry_msgs/TransformStamped.h>
+#include <geometry_msgs/PoseStamped.h>
+
+using std::string;
+
+static std::shared_ptr<tf2_ros::TransformBroadcaster> br;
+static geometry_msgs::TransformStamped body;
+
+inline void publishBody(const geometry_msgs::PoseStamped& pose)
+{
+	// Get only yaw from pose
+	tf::Quaternion q;
+	q.setRPY(0, 0, tf::getYaw(pose.pose.orientation));
+	tf::quaternionTFToMsg(q, body.transform.rotation);
+
+	body.transform.translation.x = pose.pose.position.x;
+	body.transform.translation.y = pose.pose.position.y;
+	body.transform.translation.z = pose.pose.position.z;
+	body.header.frame_id = pose.header.frame_id;
+	body.header.stamp = pose.header.stamp;
+	br->sendTransform(body);
+}
+
+void poseCallback(const geometry_msgs::PoseStamped& pose)
+{
+	publishBody(pose);
+}
+
+int main(int argc, char **argv) {
+	ros::init(argc, argv, "frames");
+	ros::NodeHandle nh, nh_priv("~");
+
+	nh_priv.param<string>("body/frame_id", body.child_frame_id, "body");
+
+	br = std::make_shared<tf2_ros::TransformBroadcaster>();
+	ros::Subscriber pose_sub = nh.subscribe("mavros/local_position/pose", 1, &poseCallback);
+	ROS_INFO("frames: ready");
+	ros::spin();
+}

clever/src/monkey

@@ -1,3 +0,0 @@
-#!/usr/bin/env bash
-
-exec /home/pi/monkey/build/monkey --port 80 --workers 1

clever/src/selfcheck.py

@@ -6,16 +6,19 @@ import re
 import traceback
 import rospy
 from std_srvs.srv import Trigger
-from sensor_msgs.msg import Image, CameraInfo, NavSatFix, Imu
-from mavros_msgs.msg import State
+from sensor_msgs.msg import Image, CameraInfo, NavSatFix, Imu, Range
+from mavros_msgs.msg import State, OpticalFlowRad
 from geometry_msgs.msg import PoseStamped, TwistStamped
+from aruco_pose.msg import MarkerArray
 
 
 # TODO: roscore is running
+# TODO: clever.service is running
+# TODO: check attitude is present
 # TODO: disk free space
 # TODO: local_origin, fcu, fcu_horiz
 # TODO: rc service
-# TODO: perform commander check in PX4
+# TODO: perform commander check, ekf2 status on PX4
 # TODO: check if FCU params setter succeed
 # TODO: selfcheck ROS service (with blacklists for checks)
 
@@ -53,29 +56,47 @@ def check_fcu():
     try:
         state = rospy.wait_for_message('mavros/state', State, timeout=3)
         if not state.connected:
-            failure('No connection to the FCU')
+            failure('No connection to the FCU (check wiring)')
     except rospy.ROSException:
-        failure('No MAVROS state')
+        failure('No MAVROS state (check wiring)')
 
 
 @check('Camera')
 def check_camera(name):
     try:
-        rospy.wait_for_message(name + '/image_raw', Image, timeout=1)
+        img = rospy.wait_for_message(name + '/image_raw', Image, timeout=1)
     except rospy.ROSException:
-        failure('No %s camera images' % name)
+        failure('%s: No images (is the camera connected properly?)', name)
+        return
     try:
-        rospy.wait_for_message(name + '/camera_info', CameraInfo, timeout=3)
+        info = rospy.wait_for_message(name + '/camera_info', CameraInfo, timeout=1)
     except rospy.ROSException:
-        failure('No %s camera camera 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)
+    if img.height != info.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('Visual position estimate')
+def check_vpe():
+    try:
+        rospy.wait_for_message('mavros/vision_pose/pose', PoseStamped, timeout=1)
+    except rospy.ROSException:
+        try:
+            rospy.wait_for_message('mavros/mocap/pose', PoseStamped, timeout=1)
+        except rospy.ROSException:
+            failure('No VPE or MoCap messages')
 
 
 @check('Simple offboard node')
@@ -93,7 +114,7 @@ def check_imu():
     try:
         rospy.wait_for_message('mavros/imu/data', Imu, timeout=1)
     except rospy.ROSException:
-        failure('No IMU data')
+        failure('No IMU data (check flight controller calibration)')
 
 
 @check('Local position')
@@ -111,9 +132,21 @@ 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 %s m/s; is the 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 %s m/s; is the 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?',
+                    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?',
+                    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?',
+                    angular.z, math.degrees(angular.z))
     except rospy.ROSException:
         failure('No velocity estimation')
 
@@ -121,11 +154,33 @@ def check_velocity():
 @check('Global position (GPS)')
 def check_global_position():
     try:
-        rospy.wait_for_message('mavros/global_position/global', NavSatFix, timeout=2)
+        rospy.wait_for_message('mavros/global_position/global', NavSatFix, timeout=1)
     except rospy.ROSException:
         failure('No global position')
 
 
+@check('Optical flow')
+def check_optical_flow():
+    # TODO:check FPS!
+    try:
+        rospy.wait_for_message('mavros/px4flow/raw/send', OpticalFlowRad, timeout=0.5)
+    except rospy.ROSException:
+        failure('No optical flow data (from Raspberry)')
+
+
+@check('Rangefinder')
+def check_rangefinder():
+    # TODO: check FPS!
+    try:
+        rospy.wait_for_message('mavros/distance_sensor/rangefinder_3_sub', Range, timeout=0.5)
+    except rospy.ROSException:
+        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')
+
+
 @check('Boot duration')
 def check_boot_duration():
     proc = Popen('systemd-analyze', stdout=PIPE)
@@ -134,7 +189,7 @@ def check_boot_duration():
     r = re.compile(r'([\d\.]+)s$')
     duration = float(r.search(output).groups()[0])
     if duration > 15:
-        failure('long Raspbian boot duration: %ss', duration)
+        failure('long Raspbian boot duration: %ss (systemd-analyze for analyzing)', duration)
 
 
 @check('CPU usage')
@@ -164,6 +219,9 @@ def selfcheck():
     check_camera('main_camera')
     check_aruco()
     check_simpleoffboard()
+    check_optical_flow()
+    check_vpe()
+    check_rangefinder()
     check_cpu_usage()
     check_boot_duration()
 

clever/src/simple_offboard.py

@@ -86,7 +86,7 @@ LOCAL_POSITION_TIMEOUT = rospy.Duration(rospy.get_param('~local_position_timeout
 NAVIGATE_AFTER_ARMED = rospy.Duration(rospy.get_param('~navigate_after_armed', True))
 TRANSFORM_TIMEOUT = rospy.Duration(rospy.get_param('~transform_timeout', 3))
 SETPOINT_RATE = rospy.get_param('~setpoint_rate', 30)
-LOCAL_FRAME = rospy.get_param('~local_frame', 'local_origin')
+LOCAL_FRAME = rospy.get_param('mavros/local_position/frame_id', 'local_origin')
 LAND_MODE = rospy.get_param('~land_mode', 'AUTO.LAND')
 LAND_TIMEOUT = rospy.Duration(rospy.get_param('~land_timeout', 2))
 DEFAULT_SPEED = rospy.get_param('~default_speed', 0.5)
@@ -131,7 +131,10 @@ BRAKE_TIME = rospy.Duration(0)
 def get_navigate_setpoint(stamp, start, finish, start_stamp, speed):
     distance = math.sqrt((finish.z - start.z)**2 + (finish.x - start.x)**2 + (finish.y - start.y)**2)
     time = rospy.Duration(distance / speed)
-    k = (stamp - start_stamp) / time
+    if time == rospy.Duration(0):
+        k = 0
+    else:
+        k = (stamp - start_stamp) / time
     time_left = start_stamp + time - stamp
 
     if BRAKE_TIME and time_left < BRAKE_TIME:
@@ -161,6 +164,9 @@ def get_publisher_and_message(req, stamp, continued=True, update_frame=True):
     ps.header.stamp = stamp
     vs.header.stamp = stamp
 
+    # don't block on setpoints publishing
+    transform_timeout = rospy.Duration(0.1) if continued else TRANSFORM_TIMEOUT
+
     if isinstance(req, (srv.NavigateRequest, srv.NavigateGlobalRequest)):
         global current_nav_start, current_nav_start_stamp, current_nav_finish
 
@@ -168,7 +174,7 @@ def get_publisher_and_message(req, stamp, continued=True, update_frame=True):
             ps.header.frame_id = req.frame_id or LOCAL_FRAME
             ps.pose.position = Point(getattr(req, 'x', 0), getattr(req, 'y', 0), req.z)
             ps.pose.orientation = orientation_from_euler(0, 0, req.yaw, axes='sxyz')
-            current_nav_finish = tf_buffer.transform(ps, LOCAL_FRAME, TRANSFORM_TIMEOUT)
+            current_nav_finish = tf_buffer.transform(ps, LOCAL_FRAME, transform_timeout)
 
             if isinstance(req, srv.NavigateGlobalRequest):
                 # Recalculate x and y from lat and lon
@@ -200,7 +206,7 @@ def get_publisher_and_message(req, stamp, continued=True, update_frame=True):
         ps.header.frame_id = req.frame_id or LOCAL_FRAME
         ps.pose.position = Point(getattr(req, 'x', 0), getattr(req, 'y', 0), req.z)
         ps.pose.orientation = orientation_from_euler(0, 0, req.yaw)
-        pose_local = tf_buffer.transform(ps, LOCAL_FRAME, TRANSFORM_TIMEOUT)
+        pose_local = tf_buffer.transform(ps, LOCAL_FRAME, transform_timeout)
 
         if isinstance(req, srv.SetPositionGlobalRequest):
             pose_local.pose.position.x, pose_local.pose.position.y = global_to_local(req.lat, req.lon)
@@ -221,8 +227,8 @@ def get_publisher_and_message(req, stamp, continued=True, update_frame=True):
         vs.header.frame_id = req.frame_id or LOCAL_FRAME
         ps.header.frame_id = req.frame_id or LOCAL_FRAME
         ps.pose.orientation = orientation_from_euler(0, 0, req.yaw)
-        pose_local = tf_buffer.transform(ps, LOCAL_FRAME, TRANSFORM_TIMEOUT)
-        vector_local = tf_buffer.transform(vs, LOCAL_FRAME, TRANSFORM_TIMEOUT)
+        pose_local = tf_buffer.transform(ps, LOCAL_FRAME, transform_timeout)
+        vector_local = tf_buffer.transform(vs, LOCAL_FRAME, transform_timeout)
 
         yaw_rate_flag = math.isnan(req.yaw)
         msg = pt
@@ -238,7 +244,7 @@ def get_publisher_and_message(req, stamp, continued=True, update_frame=True):
     elif isinstance(req, srv.SetAttitudeRequest):
         ps.header.frame_id = req.frame_id or LOCAL_FRAME
         ps.pose.orientation = orientation_from_euler(req.roll, req.pitch, req.yaw)
-        pose_local = tf_buffer.transform(ps, LOCAL_FRAME, TRANSFORM_TIMEOUT)
+        pose_local = tf_buffer.transform(ps, LOCAL_FRAME, transform_timeout)
         msg = at
         msg.orientation = pose_local.pose.orientation
         msg.thrust = req.thrust
@@ -292,8 +298,13 @@ def handle(req):
         return {'message': 'Both yaw and yaw_rate cannot be NaN'}
 
     try:
+        # check frame_id existance
+        # (for non-blocking setpoint's publishing in get_publisher_and_message)
+        stamp = rospy.get_rostime()
+        if hasattr(req, 'frame_id'):
+            tf_buffer.lookup_transform(req.frame_id or LOCAL_FRAME, LOCAL_FRAME, stamp, TRANSFORM_TIMEOUT)
+
         with handle_lock:
-            stamp = rospy.get_rostime()
             current_req = req
             current_pub, current_msg = get_publisher_and_message(req, stamp, False)
             rospy.loginfo('Topic: %s, message: %s', current_pub.name, current_msg)
@@ -377,24 +388,31 @@ def get_telemetry(req):
     frame_id = req.frame_id or LOCAL_FRAME
     stamp = rospy.get_rostime()
 
-    if pose:
-        p = tf_buffer.transform(pose, frame_id, TRANSFORM_TIMEOUT)
-        res['x'] = p.pose.position.x
-        res['y'] = p.pose.position.y
-        res['z'] = p.pose.position.z
+    transform_timeout = rospy.Duration(0.4)
+    try:
+        if pose:
+            p = tf_buffer.transform(pose, frame_id, transform_timeout)
+            res['x'] = p.pose.position.x
+            res['y'] = p.pose.position.y
+            res['z'] = p.pose.position.z
 
-        # Calculate roll pitch and yaw as Tait-Bryan angles, order z-y-x
-        res['yaw'], res['pitch'], res['roll'] = euler_from_orientation(p.pose.orientation, axes='rzyx')
+            # Calculate roll pitch and yaw as Tait-Bryan angles, order z-y-x
+            res['yaw'], res['pitch'], res['roll'] = euler_from_orientation(p.pose.orientation, axes='rzyx')
+    except:
+        pass
 
     if velocity:
-        v = Vector3Stamped()
-        v.header.stamp = velocity.header.stamp
-        v.header.frame_id = velocity.header.frame_id
-        v.vector = velocity.twist.linear
-        linear = tf_buffer.transform(v, frame_id, TRANSFORM_TIMEOUT)
-        res['vx'] = linear.vector.x
-        res['vy'] = linear.vector.y
-        res['vz'] = linear.vector.z
+        try:
+            v = Vector3Stamped()
+            v.header.stamp = velocity.header.stamp
+            v.header.frame_id = velocity.header.frame_id
+            v.vector = velocity.twist.linear
+            linear = tf_buffer.transform(v, frame_id, transform_timeout)
+            res['vx'] = linear.vector.x
+            res['vy'] = linear.vector.y
+            res['vz'] = linear.vector.z
+        except:
+            pass
 
         res['yaw_rate'] = velocity.twist.angular.z
         res['pitch_rate'] = velocity.twist.angular.y
@@ -441,21 +459,21 @@ def start_loop():
                         current_pub, current_msg = get_publisher_and_message(current_req, stamp, True,
                                                                              getattr(current_req, 'update_frame', False))
 
-                    current_msg.header.stamp = stamp
-                    current_pub.publish(current_msg)
-
-                    # For monitoring
-                    if isinstance(current_msg, PositionTarget):
-                        p = PoseStamped()
-                        p.header.frame_id = LOCAL_FRAME
-                        p.header.stamp = stamp
-                        p.pose.position = current_msg.position
-                        p.pose.orientation = orientation_from_euler(0, 0, current_msg.yaw + math.pi / 2)
-                        target_pub.publish(p)
-
                 except Exception as e:
                     rospy.logwarn_throttle(10, str(e))
 
+                current_msg.header.stamp = stamp
+                current_pub.publish(current_msg)
+
+                # For monitoring
+                if isinstance(current_msg, PositionTarget):
+                    p = PoseStamped()
+                    p.header.frame_id = LOCAL_FRAME
+                    p.header.stamp = stamp
+                    p.pose.position = current_msg.position
+                    p.pose.orientation = orientation_from_euler(0, 0, current_msg.yaw)
+                    target_pub.publish(p)
+
         r.sleep()
 
 

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/vl53l1x.py

@@ -0,0 +1,38 @@
+#!/usr/bin/env python
+# TODO: rewrite, as Python version eats 20% CPU
+
+from __future__ import division
+
+import rospy
+import VL53L1X
+from sensor_msgs.msg import Range
+
+rospy.init_node('vl53l1x')
+
+
+# range_pub = rospy.Publisher('~range', Range, queue_size=5)
+# TODO: why remmaping is not working?
+range_pub = rospy.Publisher('mavros/distance_sensor/rangefinder_3_sub', Range, queue_size=10)
+z_shift = rospy.get_param("z_shift", 0)  # TODO: move to mavros (use frame)
+
+msg = Range()
+msg.radiation_type = Range.INFRARED
+msg.field_of_view = 0.471239
+msg.min_range = 0
+msg.max_range = 4
+msg.header.frame_id = 'rangefinder'
+
+tof = VL53L1X.VL53L1X(i2c_bus=1, i2c_address=0x29)
+tof.open() # Initialise the i2c bus and configure the sensor
+tof.start_ranging(3) # Start ranging, 1 = Short Range, 2 = Medium Range, 3 = Long Range
+
+rospy.loginfo('vl53l1x: start ranging')
+
+r = rospy.Rate(14)
+while not rospy.is_shutdown():
+    msg.header.stamp = rospy.get_rostime()
+    msg.range = tof.get_distance() / 1000 + z_shift
+    range_pub.publish(msg)
+    r.sleep()
+
+tof.stop_ranging() # Stop ranging

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();
+}

deploy/generate_ros_lib

@@ -1,9 +0,0 @@
-#!/usr/bin/env bash
-
-# This script generates ros_lib library for Arduino for using with rosseial_arduino:
-# http://wiki.ros.org/rosserial_arduino/Tutorials
-# https://copterexpress.gitbooks.io/clever/content/docs/arduino.html
-
-rm -rf ros_lib
-rosrun rosserial_arduino make_libraries.py .
-tar czf clever_arudino.tar.gz ros_lib

docs/SUMMARY.md

@@ -2,7 +2,8 @@
 
 * [Введение](README.md)
 * [Глоссарий](glossary.md)
-* [Сборка](assemble.md)
+* [Сборка Клевер 2](assemble.md)
+* [Сборка Клевер 3](assemble_clever3_4in1.md)
 * [Первоначальная настройка](setup.md)
 * [Полетные режимы](modes.md)
 * [Raspberry Pi](raspberry.md)
@@ -11,6 +12,7 @@
 * [Подключение по 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)
@@ -47,4 +49,6 @@
   * [Прошивка 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,5 +1,4 @@
-Управление коптером с Arduino
-===
+# Управление коптером с Arduino
 
 Для взаимодействия с ROS-топиками и сервисами на Raspberry Pi можно использовать библиотеку [rosserial_arduino](http://wiki.ros.org/rosserial_arduino).
 
@@ -7,17 +6,23 @@
 
 Arudino необходимо установить на Клевер и подключить по USB-порту.
 
-Настройка Arduino IDE
----
+## Настройка Arduino IDE
 
-Необходимо скачать и скопировать [библиотеку ROS-сообщений Клевера](https://github.com/CopterExpress/clever_bundle/blob/master/deploy/clever_arudino.tar.gz?raw=true) (`ros_lib`) в `<папку скетчей>/libraries`.
+Для работы с ROS, Arduino необходимо понимать формат сообщений. Для этого на Clever необходимо собрать библиотеку ROS-сообщений (`ros_lib`) и скопировать в папку `<папку скетчей>/libraries`.
 
-Настройка Raspberry Pi
----
+Для сборки библиотеки на коптере необходимо выполнить следующий скрипт:
+
+```bash
+rosrun rosserial_arduino make_libraries.py .
+tar czf clever_arudino.tar.gz ros_lib
+rm -rf ros_lib
+```
+
+## Настройка Raspberry Pi
 
 Чтобы единоразово запустить программу на Arduino, можно воспользоваться командой:
 
-```
+```bash
 roslaunch clever arduino.launch
 ```
 
@@ -33,8 +38,7 @@ roslaunch clever arduino.launch
 sudo systemctl restart clever
 ```
 
-Задержки
----
+## Задержки
 
 При использовании `rosserial_arduino` микроконтроллер Arduino не должен быть заблокирован больше чем на несколько секунд (например, с использованием функции `delay`); иначе связь между Raspberry Pi и Arduino будет разорвана. 
 
@@ -52,13 +56,12 @@ while(/* условие */) {
 ```cpp
 // Задержка на 8 секунд
 for(int i=0; i<8; i++) {
-	delay(1000);
-	nh.spinOnce();
+  delay(1000);
+  nh.spinOnce();
 }
 ```
 
-Работа с Клевером
----
+## Работа с Клевером
 
 Набор сервисов и топиков аналогичен обычному набору в [simple_offboard](simple_offboard.md) и [mavros](mavros.md).
 
@@ -165,8 +168,7 @@ void loop()
 }
 ```
 
-Получение телеметрии
----
+## Получение телеметрии
 
 С Arduino можно использовать [сервис](simple_offboard.md) `get_telemetry`. Для этого надо объявить его по аналогии с сервисами `navigate` и `set_mode`:
 
@@ -201,8 +203,7 @@ getTelemetry.call(gt_req, gt_res);
 // gt_res.z - положение коптера по z
 ```
 
-Проблемы
----
+## Проблемы
 
 При использовании Arudino Nano может не хватать оперативной памяти (RAM). В таком случае в Aruino IDE будут появляться сообщения, типа:
 

docs/assemble.md

@@ -1,10 +1,8 @@
-Инструкция по сборке конструктора Клевер 2
-==========================================
+# Инструкция по сборке конструктора Клевер 2
 
 ![Clever](assets/Clevermain.png)
 
-Состав конструктора
--------------------
+## Состав конструктора
 
 ![Explosion](assets/explosion.png)
 
@@ -27,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.
@@ -37,7 +35,8 @@
 * Батарейка АА х4
 * Джампер, Bind-разъем
 
-#### Крепежные элементы
+### Крепежные элементы
+
 * Пластиковые стойки 6 мм x28.
 * Пластиковые стойки 30 мм x32.
 * Винты М3x8 x48.
@@ -54,10 +53,8 @@
 * Ножницы канцелярские х1
 * Ремешок для батареи 250 мм х1
 
+## Функционал радиопульта Flysky i6
 
-
-Функционал радиопульта Flysky i6
----------------------------
 1. Переключатель A (SwA).
 2. Переключатель B (SwB).
 3. Переключатель С (SwC).
@@ -78,10 +75,9 @@
 
 ![radio Transmitter](assets/radioTransmitter.png)
 
+## Дополнительное оборудование
 
-Дополнительное оборудование
----------------------------
-#### Данное оборудование не входит в состав конструктора Клевер 2, но оно необходимо для реализации сборочного процесса
+### Данное оборудование не входит в состав конструктора Клевер 2, но оно необходимо для реализации сборочного процесса
 
 1. Паяльник
 2. Канифоль/ Флюс (нейтральный)
@@ -94,11 +90,9 @@
 
 ![Дополнительное оборудование](assets/addEqipment.jpg)
 
-
 [Техника безопасности при пайке](tb.md)
 
-Порядок сборки
---------------
+## Порядок сборки
 
 ### Установка моторов
 
@@ -107,11 +101,13 @@
 ![Мотор brrc2205](assets/brrc2205.png)
 
 Зачистить
+
 * снять 2мм термоизоляции с конца провода не повредив медные жилы.
 
 Скрутить провода.
 
 Залудить
+
 * Нанести флюс на оголенную часть провода.
 * Покрыть припоем, используя пинцет.
 
@@ -128,17 +124,19 @@
 
 ![Вращение моторов](assets/brrc2205ondeck.png)
 
+### Залудить три контактные площадки регулятора
 
-#### Залудить три контактные площадки регулятора
 * Нанести флюс
 * Нанести припой
 
 Чтобы припой аккуратно заполнил всю площадку, необходимо прогреть площадку регулятора. Для этого нужно удерживать жало паяльника на контактной плащадке в течение 2 сек (или больше, если потребуется)
 
 ![Лужение контактных площадок регуляторов](assets/escDYSzap.png)
+
 * Повторить данную операцию для оставшихся трех регуляторов
 
-#### Припаять провода моторов к регуляторам
+### Припаять провода моторов к регуляторам
+
 Припаять ранее приготовленные провода моторов к контактным площадкам регуляторов.
 
 ![Припаять провода моторов к регуляторам](assets/solderingBrrc2205ondeckTOescDYSzap.png)
@@ -146,6 +144,7 @@
 * Повторить данную операцию для оставшихся трех регуляторов
 
 ### Монтаж разъемов питания
+
 [Статья про силовые и управляющие цепи](powerConnection.md)
 
 #### Подготовка проводов для силовых разъемов XT60
@@ -169,10 +168,7 @@
 4. Припаять красный провод к “+” контакту разъема .
 5. Нарезать термоусадку ф5 (2 отрезка по 10 мм).
 6. Надеть термоусадку ф5 на провода так, чтобы она закрывала контактные площадки проводов с XT60 .
-7. Усадить термоусадку феном.
-
-![Монтаж разъемов XT60](assets/mountxt60pinsocket.png)
-
+7. Усадить термоусадку феном. ![Монтаж разъемов XT60](assets/mountxt60pinsocket.png)
 8. Повторить процедуру для разъема XT60 socket.
 
 #### Подготовка разъема питания управляющей цепи 5В
@@ -193,15 +189,18 @@
 ![Предпаячная проверка](assets/startPDBtest.jpg)
 
 Прозвонить следующие цепи на НЕЗАМКНУТОСТЬ (отсутствие звукового сигнала мультиметра):
+
 * “BAT+” и “BAT-”
 * “12V” и “GND”
 * “5V” и “GND”
 
 Прозвонить следующие цепи на ЗАМКНУТОСТЬ (появление звукового сигнала мультиметра):
+
 * “BAT-” c каждым контактом, обозначенным “-” и “GND”
 * “BAT+”, с каждым контактом, обозначенным “+”
 
 #### Залудить контактные площадки платы питания
+
 1. [Залудить*](zap.md) контактные площадки платы питания.
 2. С помощью мультиметра проверить отсутствие контактного замыкания на плате (прозвонить)
 
@@ -210,15 +209,18 @@
 Чтобы припой аккуратно заполнил всю площадку, необходимо её прогреть. Для этого нужно удерживать жало паяльника на контактной плащадке в течение 2 сек (или больше, если потребуется)
 
 #### Пайка силового разъема питания XT60
+
 Припаять разъем для АКБ, соблюдая полярность на контактных площадках.
 
 ![Пайка XT60 на PDB](assets/solderingxt60socketTOpdb.png)
 
 ВАЖНО о полярности
+
 * красный провод - это “+”
 * черный провод - это “-”
 
 #### Пайка разъема питания управляющей цепи 5В
+
 Припаять разъем 5В, соблюдая полярность на контактных площадках.
 (на изображении: красный провод - это питание “+”)
 
@@ -227,6 +229,7 @@
 ### Монтаж отсека АКБ
 
 #### Подготовка перемычек (3 шт.)
+
 ![Перемычка](assets/jumper.png)
 
 * Отрезать силовой провод длиной 2 см.
@@ -237,60 +240,49 @@
 * Прозвонить мультиметром. В случае необходимости зачистить наждачной бумагой.
 
 #### Подготовка отсека АКБ
+
 ![Подготовка отсека АКБ](assets/casebattery.png)
+
 * Приклеить наклейки с разметкой внутрь отсека АКБ, в соответствии с полярностью.
 * Приклеить ленту из скотча на дно отсека.
 
-
 ### Монтаж платы распределения питания
 
-* Установить плату питания на раму винтами М3х8 и пластиковыми гайками.
-![Установка платы PDB](assets/mountPDB.png)
-
-* ВАЖНО
-Стрелочка на плате направлена в сторону носового выреза
+* Установить плату питания на раму винтами М3х8 и пластиковыми гайками. ![Установка платы PDB](assets/mountPDB.png)
+    > **ВАЖНО** Стрелочка на плате направлена в сторону носового выреза
 ![Установка платы PDB](assets/topviewmountPDB.png)
 
-
 #### Монтаж элементов
 
-1. Установить гайки в пластиковые держатели
-![Монтаж пластиковых держателей](assets/holderLegs.png)
-
+1. Установить гайки в пластиковые держатели. ![Монтаж пластиковых держателей](assets/holderLegs.png)
 2. Установить лучи на раму винтами М3х16
-*Лучи устанавливаются поверх рамы
-*Пластиковые держатели устанавливаются снизу рамы
-![Монтаж лучей](assets/mountBeams.png)
-
-3. Расположение моторов
-Проверить расположение моторов (моторы с черной гайкой в левом верхнем углу и в правом нижнем).
-![Расположение моторов](assets/motorsTopview.png)
-
-4. Продеть силовые провода регуляторов в отверстия.
-![силовые провода моторов](assets/escWires.png)
-
+    * Лучи устанавливаются поверх рамы
+    * Пластиковые держатели устанавливаются снизу рамы. ![Монтаж лучей](assets/mountBeams.png)
+3. Расположение моторов. Проверить расположение моторов (моторы с черной гайкой в левом верхнем углу и в правом нижнем). ![Расположение моторов](assets/motorsTopview.png)
+4. Продеть силовые провода регуляторов в отверстия. ![силовые провода моторов](assets/escWires.png)
 
 #### Пайка силовой цепи платы питания
+
 Припаять силовые провода регуляторов к плате питания, соблюдая полярность.
+
 ![Пайка силовых проводов на PDB](assets/solderingPowerwires.png)
 
 ВАЖНО о полярности
-*красный провод - это “+”
-*черный провод - это “-”
 
+* красный провод - это “+”
+* черный провод - это “-”
 
 ### Сопряжение приемника и пульта
-1. Подключить радиоприемник к разъему 5В. В любой разъем, GND внизу. На схеме питание обозначено как 5V
-![Подключение питания приемника](assets/receiver5V.png)
-3. Подключить АКБ.
-Светодиод на радиоприемнике должен мигать.
-![Подключение АКБ](assets/connectBattery.png)
 
+1. Подключить радиоприемник к разъему 5В. В любой разъем, GND внизу. На схеме питание обозначено как 5V ![Подключение питания приемника](assets/receiver5V.png)
+2. Подключить АКБ. Светодиод на радиоприемнике должен мигать. ![Подключение АКБ]
 
 #### БЕЗОПАСНОСТЬ при работе с АКБ
+
 ![БЕЗОПАСНОСТЬ при работе с АКБ](assets/safetyPower.png)
 
 #### Включение радиопульта
+
 1. Вставить джампер в B/VCC радиоприемника (замкнуть "землю" и "сигнал")
 2. На пульте зажать кнопку BIND KEY.
 3. Включить пульт (перещелкнуть POWER, BIND KEY не отпускаем).
@@ -302,63 +294,42 @@
 ![Подключение питания приемника](assets/connectingRadio.png)
 
 [Мануал по неисправностям радиоаппаратуры](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.png)
-
-9. Если необходимо изменить направление вращения, то меняем любые два фазных провода мотора (нужно перепаять).
-
-![Перепайка фазных проводов](assets/resolderingESC.png)
-
+8. Проверить направления вращения мотора по схеме. ![Проверка вращения моторов](assets/testMotors.png)
+9. Если необходимо изменить направление вращения, то меняем любые два фазных провода мотора (нужно перепаять). ![Перепайка фазных проводов](assets/resolderingESC.png)
 
 ### Монтаж радиоприемника
+
 1. Установить пластиковые стойки 30 мм на раму винтами М3х8.
-2. Разъем питания 5В продеть в прорезь.
-![Установка стоек и прорезь](assets/mountReceiverStud.png)
-
-3. Приемник прикрепить к нижней дополнительной раме, используя двухсторонний скотч и ориентируясь на гравировку. Антенны направлены вперед.
-
- ![Установка радиоприемника на деку](assets/mountReceiverDeck.png)
-
-4. Установить 3х проводной шлейф в канал PPM / CH1.
-![Подключение радиоприемника](assets/receiverPPM.png)
-
+2. Разъем питания 5В продеть в прорезь. ![Установка стоек и прорезь](assets/mountReceiverStud.png)
+3. Приемник прикрепить к нижней дополнительной раме, используя двухсторонний скотч и ориентируясь на гравировку. Антенны направлены вперед. ![Установка радиоприемника на деку](assets/mountReceiverDeck.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)
 
-#### Установка полетного контроллера Pixhawk
-1. Клеим 2х сторонний скотч по углам полетного контроллера
- ![Полетный контроллер](assets/pixhawk.png)
+#### Установка полетного контроллера PixHawk
 
-##### ВАЖНО
-При работе моторов возникают вибрации, отрицательно влияющие на показания датчиков полетного контроллера Pixhawk. Чтобы избежать этого эффекта, количество слоев двустороннего скотча
+1. Клеим 2х сторонний скотч по углам полетного контроллера. ![Полетный контроллер](assets/pixhawk.png)
+    > **ВАЖНО** При работе моторов возникают вибрации, отрицательно влияющие на показания датчиков полетного контроллера PixHawk. Чтобы избежать этого эффекта, количество слоев двустороннего скотча
 лучше увеличить до 4-5.
-
-2. Установить  полетный контроллер в центр рамы
-
- ![Полетный контроллер](assets/topviewpixhawk.png)
-##### ВАЖНО
-Стрелки на раме и pixhawk должны быть сонаправлены
+2. Установить  полетный контроллер в центр рамы. ![Полетный контроллер](assets/topviewpixhawk.png)
+    > **ВАЖНО** Стрелки на раме и PixHawk должны быть сонаправлены
 
 #### Подключение полетного контроллера по схеме
 
@@ -368,47 +339,31 @@
 
  ![Подключение полетного контроллера](assets/connectionPixhawk.png)
 
-
 ### Сборка регуляторов
-1. Клеим 2х сторонний скотч на основание защитного бокса регуляторов
-![Скотч на бокс регулей](assets/escCase.png)
-
-2. Укладываем регуляторы в защитные боксы. Крепим полученную сборку к лучам рамы.
-![Вид сверху с боксами для  регулей](assets/topESCcaseview.png)
 
+1. Клеим 2х сторонний скотч на основание защитного бокса регуляторов. ![Скотч на бокс регулей](assets/escCase.png)
+2. Укладываем регуляторы в защитные боксы. Крепим полученную сборку к лучам рамы. ![Вид сверху с боксами для  регулей](assets/topESCcaseview.png)
 
 ### Установка защиты
-1. Закрепить нижнюю защиту винтами М3х16 на лучах рамы
-![Установка лучевой защиты](assets/lowsafeDeck.png)
-
-2. Закрепить ножки к пластиковым держателям винтами М3х16
-![Установка ножек](assets/safeLegs.png)
-
-3. Закрепить стойки 30 мм в отверстия нижней защиты винтами М3х12
-![Установка нижней радиальной защиты](assets/safelowRadial.png)
-
-4. Закрепить верхнюю защиту винтами М3х12
-![Установка верхней радиальной защиты](assets/safehighRadial.png)
 
+1. Закрепить нижнюю защиту винтами М3х16 на лучах рамы. ![Установка лучевой защиты](assets/lowsafeDeck.png)
+2. Закрепить ножки к пластиковым держателям винтами М3х16. ![Установка ножек](assets/safeLegs.png)
+3. Закрепить стойки 30 мм в отверстия нижней защиты винтами М3х12. ![Установка нижней радиальной защиты](assets/safelowRadial.png)
+4. Закрепить верхнюю защиту винтами М3х12. ![Установка верхней радиальной защиты](assets/safehighRadial.png)
 
 ### Монтаж отсека АКБ
 
-Необходимые компоненты
+Необходимые компоненты:
+
 * Винты М3х12 (4 шт)
 * Гайки М3 (4 шт)
 * Рама дополнительная (1 шт)
 * Батарейный отсек (1 шт)
 
-1. Прикрепить батарейный отсек на верхнюю дополнительную раму винтами М3х12 и гайками.
-![Монтаж отсека АКБ](assets/mountHolder.png)
-
-2. Прикрепить верхнюю дополнительную раму на стойки винтами М3х8.
-
-![Монтаж отсека АКБ](assets/isoViewmountHolder.png)
-
+1. Прикрепить батарейный отсек на верхнюю дополнительную раму винтами М3х12 и гайками. ![Монтаж отсека АКБ](assets/mountHolder.png)
+2. Прикрепить верхнюю дополнительную раму на стойки винтами М3х8. ![Монтаж отсека АКБ](assets/isoViewmountHolder.png)
 3. Установить АКБ в отсек.
 
-
 ### Монтаж антенн
 
 1. Крепим антенны на 2х сторонний скотч или изоленту, а усики продеваем в передние отверстия верхней дополнительной рамы.
@@ -417,16 +372,11 @@
 
 Коптер готов к настройке!
 
-
 ## Безопасность при сборке и настройке
 
 1. Снять пропеллеры.“Все наземные операции производить со снятыми пропеллерами. Устанавливать пропеллеры на моторы только перед полётом.”
-
-2. Отключить аккумулятор. Держать питание выключенным.
-“Сборку, настройку и ремонт производить с отключенным питанием. Подключать питание только для тестирования электронных компонентов коптера. После тестирования перед другими работами питание сразу отключить.”
-
-3. Позвать на помощь
-“Если при выполнении работ возникли какие-либо проблемы, необходимо обратиться к преподавателю или учителю, а не пытаться решить проблему самостоятельно.”
+2. Отключить аккумулятор. Держать питание выключенным. “Сборку, настройку и ремонт производить с отключенным питанием. Подключать питание только для тестирования электронных компонентов коптера. После тестирования перед другими работами питание сразу отключить.”
+3. Позвать на помощь. “Если при выполнении работ возникли какие-либо проблемы, необходимо обратиться к преподавателю или учителю, а не пытаться решить проблему самостоятельно.”
 
 ![Безопасность при сборке](assets/safetybyassem.png)
 
@@ -436,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/drugoe.md

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

docs/lessons.md

@@ -49,3 +49,8 @@
 Часть 6
 
 <iframe width="560" height="315" src="https://www.youtube.com/embed/v00oNVzwICg" frameborder="0" allow="autoplay; encrypted-media" allowfullscreen></iframe>
+
+Автономные полеты
+
+<iframe width="560" height="315" src="https://www.youtube.com/embed/WvIlRG7ShWA" frameborder="0" allow="autoplay; encrypted-media" allowfullscreen></iframe>
+

docs/modes.md

@@ -41,8 +41,8 @@ PX4
 
 Основные используемые пакеты MAVLink:
 
-* MAV_CMD_COMPONENT_ARM_DISARM
-* [SET_POSITION_TARGET_LOCAL_NED](https://pixhawk.ethz.ch/mavlink/#SET_POSITION_TARGET_LOCAL_NED)
-* [SET_ATTITUDE_TARGET](https://pixhawk.ethz.ch/mavlink/#SET_ATTITUDE_TARGET)
+* [MAV_CMD_COMPONENT_ARM_DISARM](https://mavlink.io/en/messages/common.html#MAV_CMD_COMPONENT_ARM_DISARM)
+* [SET_POSITION_TARGET_LOCAL_NED](https://mavlink.io/en/messages/common.html#SET_POSITION_TARGET_LOCAL_NED)
+* [SET_ATTITUDE_TARGET](https://mavlink.io/en/messages/common.html#SET_ATTITUDE_TARGET)
 
 См.: [автономные полеты коптера в режиме OFFBOARD](simple_offboard.md).

docs/optical_flow.md

@@ -0,0 +1,57 @@
+# Использование Optical Flow
+
+> **Warning** Данная функция является **экспериментальной** и включена в образ с версии v0.11.4.
+
+При использовании технологии Optical Flow возможен полет в режиме POSCTL и автономные полеты по камере, направленной вниз, засчет измерения сдвигов текстуры поверхности пола.
+
+## Включение
+
+А данный момент для использования Optical Flow необходима [кастомная прошивка PX4](https://yadi.sk/d/KaxaIhohu4V8XA).
+
+Необходимо использования дальномера. При использовании дальномера STM vl53l1x, необходимо подключить его к Raspberry Pi по I2C и включить его в `~/catkin_ws/src/clever/clever/launch/clever.launch`:
+
+```xml
+<arg name="vl53l1x" default="true"/>
+```
+
+Необходимо включить Optical Flow:
+
+```xml
+<arg name="optical_flow" default="true"/>
+```
+
+В `main_camera.launch` должен быть выставлен корректный фрейм камеры.
+
+Рекомендуемые параметры PX4:
+
+* `SYS_MC_EST_GROUP` – 2 (EKF2).
+* `EKF2_AID_MASK` – use optical flow.
+* `EKF2_OF_DELAY` – 0.
+* `EKF2_OF_QMIN` – 20.
+* `EKF2_HGT_MODE` – range sensor (ремменд.).
+
+## Полет в POSCTL
+
+Настройте POSCTL как один из полетных режимов PX4. Переведите в режим POSCTL.
+
+## Автономный полет
+
+Автономный полет возможен с использованием модуля [simple_offboard](simple_offboard.md).
+
+## Troubleshooting
+
+При появлении в QGC ошибок типа `EKF INTERNAL CHECKS` попробуйте перезагрузить ekf2. Для этого наберите в MAVLink-консоли:
+
+```nsh
+ekf2 stop
+ekf2 start
+```
+
+Если коптер будет сильно уплывать по рысканью, попробуйте следующее:
+
+* Перекалибровать гироскопы
+* Перекалибровать магнитометр
+* Попробовать разные значения параметра `EKF2_MAG_TYPE`, который определяет, каким образом данные с магнитометра используются в EKF2.
+* Изменять значения параметров `EKF2_MAG_NOISE`, `EKF2_GYR_NOISE`, `EKF2_GYR_B_NOISE`.
+
+Если коптер уплывает по высоте, попробуйте также выставить `MPC_ALT_MODE` = 2 (Terrain following).

docs/px4_parameters.md

@@ -0,0 +1,70 @@
+# Параметры PX4
+
+Основная статья: https://dev.px4.io/en/advanced/parameter_reference.html
+
+> **Note** Это описание некоторых, наиболее важных параметров PX4 по состоянию на версию 1.8.0. Полный список см. по ссылке выше.
+
+Для изменения параметров PX4 можно воспользоваться программой QGroundControl, [подключившись к Клеверу по Wi-Fi](gcs_bridge.md).
+
+## Основные параметры
+
+Наиболее важные параметры вынесены в этот параграф.
+
+`SYS_MC_EST_GROUP` – выбор модуля estimator'а.
+
+Это группа модулей, которая вычисляет текущее состояние (state) коптера, используя показания с датчиков. В состояние коптера входит:
+
+* угловая скорость коптера – pitch_rate, roll_rate, yaw_rate;
+* ориентация коптера (в локальной системе координат) – pitch (тангаж), roll (крен), yaw (рысканье) (одно из представлений);
+* позиция коптера (в локальной системе координат) – x, y, z;
+* скорость коптера (в локальной системе координат) – vx, vy, vz;
+* глобальные координаты коптера – lattitude, longitude, altitude;
+* высота над поверхностью;
+* другие параметры (дрейф гироскопов, скорость ветра и пр.).
+
+`SYS_AUTOCONFIG` – сброс всех параметров (при установке в значение 1).
+
+## EKF2
+
+`EKF2_AID_MASK` – выбор датчиков, которые используются EKF2 для вычисления состояния коптера.
+
+`EKF2_HGT_MODE` – основной источник данных о высоте (z в локальной системе координт):
+
+* 0 – давление с барометра.
+* 1 – GPS.
+* 2 – дальномер (например, vl53l1x).
+* 3 – данные с VPE.
+
+Вариант 2 является наиболее точным, но его корректно использовать, только если поверхность, над которой летает котер – плоская. В противном случае начало координат по Z будет двигаться вверх и вниз с изменением высоты поверхности.
+
+## Multicopter Position Control (полет по позиции)
+
+Данные параметры настраивают полет коптера по позиции (режимы POSCTL, OFFBOARD, AUTO).
+
+`MPC_THR_HOVER` – газ висения. Данный параметр наобходимо установить на примерный процент газа, необходимый для того, чтобы коптер удерживал высоту. Если коптер имеет тенденцию набирать или терять высоту в режиме удержания высоты – можно уменьшить или увеличить это значение.
+
+`MPC_XY_P` – коэффициент *P* регулятора по позиции. Этот параметр влияет на то, насколько резко коптер будет выполнять заданные команды по позиции. Слишком большое значение может вызвать перестрелы.
+
+`MPC_XY_VEL_P` – коэффициент *P* регулятора по скорости. Данный параметр также влияет на точность и резкость выполнения коптером заданной позиции. При слишком большом значении возможны перестрелы.
+
+`MPC_XY_VEL_MAX` – максимальная горизонтальная скорость в режимах POSCTL, OFFBOARD, AUTO.
+
+`MPC_Z_P`, `MPC_Z_VEL_P` – коэффициенты *P* регуляторов по вертикальной позиции и скорости. Влияют на удерживание коптером необходимой высоты.
+
+`MPC_LAND_SPEED` – вертикальная скорость посадки в режиме LAND.
+
+## LPE + Q attitude estimator
+
+Данные параметры настраивают поведение модулей `lpe` и `q`, которые вычисляют состояние (ориентацию, позицию) коптера. Эти параметры применяются **только** если параметр `SYS_MC_EST_GROUP` установлен в значение `1` (local_position_estimator, attitude_estimator_q)
+
+TODO
+
+## Commander
+
+Преарм-чеки, переключение режимов и состояний коптера.
+
+## Sensors
+
+Включение, выключение и настройка различных датчков.
+
+TODO

docs/rc.md

@@ -16,6 +16,8 @@
 
 > **Note** Мобильный пульт конфликтует с реальной аппаратурой радиоуправления. Во время использования мобильного пульта она должна быть выключена.
 
+> **Warning** Открытое соединение QGroundControl или rviz пересылает большие объемы данных по Wi-Fi, что может негативно сказаться на отзывчивости мобильного пульта. Рекомендуется не использовать эти приложения одновременно с ним.
+
 Установите [образ Clever на RPi](microsd_images.md). Для работы приложения параметры `rosbridge` и `rc` в launch-файле (`~/catkin_ws/src/clever/clever/launch/clever.launch`) должны быть включены:
 
 ```xml

docs/rviz.md

@@ -1,11 +1,13 @@
-Использование rviz
+Использование rviz и rqt
 ===
 
-![](assets/Снимок экрана 2017-11-28 в 23.50.36.png)
+![](assets/rviz.png)
 
 Инструмент [rviz](http://wiki.ros.org/rviz) позволяет в реальном времени визуализировать на 3D-сцене все компоненты роботехнической системы — системы координат, движущиеся части, показания датчиков, изображения с камер.
 
-Для использования rviz необходим компьютер с ОС Ubuntu Linux (либо виртуальная машина, например [Parallels Desktop Lite](https://itunes.apple.com/ru/app/parallels-desktop-lite/id1085114709?mt=12) или [VirtualBox](https://www.virtualbox.org)).
+[rqt](http://wiki.ros.org/rqt) – это набор GUI для анализа и контроля ROS-систем. Например, `rqt_image_view` позволяет просматривать топики с изображениями, `rqt_multiplot` – строить графики по значениям в топиках и т. д.
+
+Для использования rviz и rqt необходим компьютер с ОС Ubuntu Linux (либо виртуальная машина, например [Parallels Desktop Lite](https://itunes.apple.com/ru/app/parallels-desktop-lite/id1085114709?mt=12) или [VirtualBox](https://www.virtualbox.org)).
 
 На него необходимо установить пакет `ros-kinetic-desktop-full` или `ros-kinetic-desktop`, используя [документацию по установке](http://wiki.ros.org/kinetic/Installation/Ubuntu).
 
@@ -24,14 +26,34 @@ ROS_MASTER_URI=http://192.168.11.1:11311 rviz
 export ROS_IP=192.168.11.1
 ```
 
-Использование
+Использование rviz
 ---
 
 В качестве reference frame рекомендуется установить фрейм `local_origin`. Для визуализации коптера можно добавить визуализационные маркеры из топика `/vehicle_markers`. Можно просмотреть картинку с дополненной реальностью из топика основной камеры `/main_camera/image_raw`.
 
 Axis или Grid настроенный на фрейм `aruco_map` будут визуализировать расположение [карты ArUco-меток](aruco.md).
 
-Рекомендуется также установка набора дополнительных полезных плагинов [jsk_rviz_plugins](https://jsk-docs.readthedocs.io/en/latest/jsk_visualization/doc/jsk_rviz_plugins/index.html). Это набор позволяет визуализировать топики типа `TwistStamped` (скорость), `CameraInfo`, `PolygonArray` и многое другое. Для установки используйте команду:
+Запуск инструментов rqt
+---
+
+![](assets/rqt.png)
+
+Для запуска rqt для мониторинга состояния Клевера используйте команду:
+
+```bash
+ROS_MASTER_URI=http://192.168.11.1:11311 rqt
+```
+
+Пример запуск конкретного плагина (`rqt_image_view`):
+
+```bash
+ROS_MASTER_URI=http://192.168.11.1:11311 rqt_image_view
+```
+
+jsk_rviz_plugins
+---
+
+Рекомендуется также установка набора дополнительных полезных плагинов для rviz [jsk_rviz_plugins](https://jsk-docs.readthedocs.io/en/latest/jsk_visualization/doc/jsk_rviz_plugins/index.html). Это набор позволяет визуализировать топики типа `TwistStamped` (скорость), `CameraInfo`, `PolygonArray` и многое другое. Для установки используйте команду:
 
 ```bash
 sudo apt-get install ros-kinetic-jsk-visualization

docs/ssh.md

@@ -13,4 +13,6 @@ ssh pi@192.168.11.1
 
 Для доступа по SSH из Windows можно использовать [PuTTY](https://www.chiark.greenend.org.uk/~sgtatham/putty/latest.html).
 
+Также можно получить доступ по SSH со смартфона с помощью приложения [Termius](https://www.termius.com).
+
 Подробнее: https://www.raspberrypi.org/documentation/remote-access/ssh/README.md

docs/testovoe-opisanie-klevera-po-shablonu-robotsrosorggapter.md

@@ -0,0 +1,2 @@
+https://robots.ros.org/gapter/
+

image_builder/Jenkinsfile

@@ -1,35 +0,0 @@
-pipeline {
-  agent any
-  parameters {
-    string(name: 'GWBT_REF', defaultValue: "master")
-    string(name: 'GWBT_URL', defaultValue: "https://github.com/CopterExpress/clever.git")
-    string(name: 'GWBT_FILE', defaultValue: "")
-    string(name: 'IMAGE_NAME', defaultValue: "\$(cat ${GWBT_FILE} | jq '.repository.name' -r)-${params.GWBT_REF}.img")
-    string(name: 'GWBT_EVENT', defaultValue: 'release')
-    booleanParam(name: 'ONLY_PUBLISH', defaultValue: false, description: 'ONLY PUBLISH')
-    string(name: 'BUILD_DIR', defaultValue: '/mnt/hdd_builder/workspace', description: 'Build workspace')
-  }
-  environment {
-    DEBIAN_FRONTEND = 'noninteractive'
-    LANG = 'C.UTF-8'
-    LC_ALL = 'C.UTF-8'
-  }
-  stages {
-    stage('Build image') {
-      when { not { expression { return params.ONLY_PUBLISH } } }
-      steps {
-        build job: 'CopterExpress-clever-build', parameters: [[$class: 'StringParameterValue', name: 'IMAGE_NAME', value: "${params.IMAGE_NAME}"], [$class: 'StringParameterValue', name: 'IMAGE_VERSION', value: "${params.GWBT_REF}"], [$class: 'StringParameterValue', name: 'GWBT_REF', value: "${params.GWBT_REF}"], [$class: 'StringParameterValue', name: 'GWBT_URL', value: "${params.GWBT_URL}"]]
-      }
-    }
-    stage('Publish image') {
-      when { environment name: 'GWBT_EVENT', value: 'release' }
-      environment {
-        CONFIG_FILE = "${params.BUILD_DIR}/coex-ci.json"
-        YA_SCRIPT = "$WORKSPACE/image_builder/yadisk.py"
-      }
-      steps {
-        sh "$WORKSPACE/image_builder/image_config.sh publish_image ${params.BUILD_DIR} ${params.IMAGE_NAME} ${YA_SCRIPT} ${CONFIG_FILE} \$(cat ${params.GWBT_FILE} | jq '.release.id' -r) \"\$(cat ${params.GWBT_FILE} | jq '.release.body' | sed 's/\"//' | rev | sed 's/\"//' | rev)\""
-      }
-    }
-  }
-}

image_builder/README.md

@@ -1,89 +0,0 @@
-## Setup your builder
-
-1. Install requirements
-```(bash)
-sudo apt-get install unzip zip git python-pip jq curl
-sudo pip install YaDiskClient
-```
-2. Mount HDD
-```bash
-nano /etc/fstab
-```
-```
-proc            /proc           proc    defaults          0       0
-PARTUUID=37665771-01  /boot           vfat    defaults          0       2
-PARTUUID=37665771-02  /               ext4    defaults,noatime  0       1
-# a swapfile is not a swap partition, no line here
-#   use  dphys-swapfile swap[on|off]  for that
-/dev/sdb1       none                    swap    sw              0       0
-/dev/sdb2       /mnt/hdd_system         ext4    defaults,acl    0       0
-/dev/sdb3       /mnt/hdd_builder        ext4    defaults,acl    0       0
-```
-
-3. Enable swap on HDD
-> TODO:
-
-And disable `dphys-swapfile`
-```(bash)
-sudo systemctl stop dphys-swapfile
-sudo systemctl disable dphys-swapfile
-```
-
-3. Create /mnt/hdd_builder/workspace/coex-ci.json
-```(json)
-{
-    "yadisk":
-    {
-        "login":"LOGIN",
-        "password":"PASS",
-        "server_dir":"/clever_images"
-    },
-    "github":
-    {
-        "login":"LOGIN",
-        "password":"PASS",
-        "url":"https://api.github.com/repos/CopterExpress/clever/releases/"
-    }
-}
-```
-6. Add webhook to release on your github project
-> TODO
-
-9. Install Jenkins
-> Manual https://www.digitalocean.com/community/tutorials/how-to-install-jenkins-on-ubuntu-16-04
-
-10. Change user & group invoked Jenkins
-```(bash)
-sudo sed -i 's/JENKINS_USER=$NAME/JENKINS_USER=root/' /etc/default/jenkins
-sudo sed -i 's/JENKINS_GROUP=$NAME/JENKINS_GROUP=root/' /etc/default/jenkins
-```
-11. Install Jenikins plugins
-> Pipeline, Git SCM
-
-12. Create Jenkins pipeline job
-> TODO
-
-13. Configure Jenkins
-> TODO: Matrix autorization, GIT Token
-
-13. Add Jenkins service to autostart
-```(bash)
-sudo systemctl enable jenkins
-```
-
-14. Start service
-```(bash)
-sudo systemctl start jenkins
-```
-
-## Requirements
-
-* Jenkins (BlueOcean plugin, optional)
-
-## Troubleshooting
-
-If JDK not installed:
-
-```bash
-sudo apt-get install default-jdk
-```

image_builder/autosizer.sh

@@ -1,103 +0,0 @@
-#!/bin/bash
-
-set -e
-
-if [ $(whoami) != "root" ]; then
-  echo \
-  && echo "********************************************************************" \
-  && echo "******************** This should be run as root ********************" \
-  && echo "********************************************************************" \
-  && echo \
-  && exit 1
-fi
-
-if [[ -z $1 ]]; then
-  echo "================================================================================"
-  echo -e "\033[0;31m\033[1mAutomatic Image file resizer\033[0m\033[0m"
-  echo -e "\033[0;31m\033[1mDescription:\033[0m\033[0m This script shrink your image to 10MiB free space"
-  echo -e "if you didn't set FREE_SPACE in MiB (see usage below)."
-  echo -e "\033[0;31m\033[1mAuthors:\033[0m\033[0m Artem Smirnov @urpylka, SirLagz"
-  echo
-  echo -e "\033[0;31m\033[1mUsage:\033[0m\033[0m ./autosizer.sh PATH_TO_IMAGE FREE_SPACE"
-  echo
-  echo -e "\033[0;31m\033[1mRequirements:\033[0m\033[0m parted, losetup, e2fsck, resize2fs, bc, truncate"
-  echo "================================================================================"
-  exit 0
-fi
-
-echo "================================================================================"
-strImgFile=$1
-echo -e "\033[0;31m\033[1mPath to image: $strImgFile\033[0m\033[0m"
-echo "================================================================================"
-
-if [[ ! -e $strImgFile ]]; then
-  echo -e "\033[0;31m\033[1mError: File doesn't exist\033[0m\033[0m"
-  echo
-  exit 1
-fi
-
-echo "================================================================================"
-partinfo=`parted -m $strImgFile unit B print`
-echo -e "\033[0;31m\033[1mPartition information:\033[0m\033[0m\n$partinfo"
-echo "================================================================================"
-
-partnumber=`echo "$partinfo" | grep ext4 | awk -F: '{ print $1 }'`
-echo -e "\033[0;31m\033[1mPartition number: $partnumber\033[0m\033[0m"
-echo "================================================================================"
-
-partstart=`echo "$partinfo" | grep ext4 | awk -F: '{ print substr($2,0,length($2)-1) }'`
-echo -e "\033[0;31m\033[1mPartition start: $partstart (bytes)\033[0m\033[0m"
-echo "================================================================================"
-
-loopback=`losetup -f --show -o $partstart $strImgFile`
-echo -e "\033[0;31m\033[1mLoopback device: $loopback\033[0m\033[0m"
-echo "================================================================================"
-
-set +e
-e2fsck -fvy $loopback
-set -e
-
-echo "================================================================================"
-minsize=`resize2fs -P $loopback | awk -F': ' '{ print $2 }'`
-#minsize=`resize2fs -P $loopback 2> /dev/null | awk -F': ' '{ print $2 }'`
-echo -e "\033[0;31m\033[1mMinsize: $minsize (4KiB)\033[0m\033[0m"
-echo "================================================================================"
-
-# Default add 10MiB free space to image, if $2 doesn't set
-FREE_SPACE=${2:-10}
-
-FREE_SPACE=$(($FREE_SPACE*1024*1024/4096))
-
-minsize=`echo "$minsize+$FREE_SPACE" | bc`
-echo -e "\033[0;31m\033[1mMinsize + $FREE_SPACE (4KiB): $minsize (4KiB)\033[0m\033[0m"
-echo "================================================================================"
-
-resize2fs -p $loopback $minsize
-sleep 1
-losetup -d $loopback
-
-echo "================================================================================"
-partnewsize=`echo "$minsize * 4096" | bc`
-echo -e "\033[0;31m\033[1mNew size of part: $minsize (4KiB) = $partnewsize (bytes)\033[0m\033[0m"
-echo "================================================================================"
-
-newpartend=`echo "$partstart + $partnewsize" | bc`
-echo -e "\033[0;31m\033[1mNew end of part (Part start + part new size):\033[0m\033[0m"
-echo -e "\033[0;31m\033[1m$partstart (bytes) + $partnewsize (bytes) = $newpartend (bytes)\033[0m\033[0m"
-echo "================================================================================"
-
-part1=`parted $strImgFile rm 2`
-echo "================================================================================"
-part2=`parted $strImgFile unit B mkpart primary $partstart $newpartend`
-
-echo "================================================================================"
-endresult=`parted -m $strImgFile unit B print free | tail -1 | awk -F: '{ print substr($2,0,length($2)-1) }'`
-echo -e "\033[0;31m\033[1mSize of result image: $endresult (bytes)\033[0m\033[0m"
-echo "================================================================================"
-
-truncate -s $endresult $strImgFile
-
-echo "================================================================================"
-partinfo=`parted -m $strImgFile unit B print`
-echo -e "\033[0;31m\033[1mPartition information:\033[0m\033[0m\n$partinfo"
-echo "================================================================================"

image_builder/build.Jenkinsfile

@@ -1,94 +0,0 @@
-pipeline {
-  agent any
-  parameters {
-    string(name: 'IMAGE_NAME', defaultValue: 'clever_noname.img', description: 'Output image file name')
-    string(name: 'GWBT_REF', defaultValue: 'master', description: 'Checkout ref-param')
-    string(name: 'IMAGE_VERSION', defaultValue: 'no_version', description: 'Image version')
-
-    string(name: 'BUILD_DIR', defaultValue: '/mnt/hdd_builder/workspace', description: 'Build workspace')
-
-    string(name: 'RPI_DONWLOAD_URL', defaultValue: 'http://director.downloads.raspberrypi.org/raspbian_lite/images/raspbian_lite-2018-06-29/2018-06-27-raspbian-stretch-lite.zip')
-    // TODO: Add mirrorparameters
-
-    string(name: 'GWBT_URL', defaultValue: 'https://github.com/CopterExpress/clever.git')
-
-    // Experimental function
-    booleanParam(name: 'SHRINK', defaultValue: true, description: 'SHRINK IMAGE')
-    booleanParam(name: 'DISCOVER_ROS_PACKAGES', defaultValue: false, description: 'DISCOVER ROS PACKAGES')
-  }
-  environment {
-    DEBIAN_FRONTEND = 'noninteractive'
-    LANG = 'C.UTF-8'
-    LC_ALL = 'C.UTF-8'
-  }
-  stages {
-    stage('Get image') {
-      steps {
-        sh "$WORKSPACE/image_builder/image_config.sh get_image ${params.BUILD_DIR} ${params.RPI_DONWLOAD_URL} ${params.IMAGE_NAME}"
-      }
-    }
-    stage('Resize FS') {
-      environment {
-        SIZE = '7G'
-      }
-      steps {
-        sh "$WORKSPACE/image_builder/image_config.sh resize_fs ${params.BUILD_DIR}/${params.IMAGE_NAME} $SIZE"
-      }
-    }
-    stage('Initialize image') {
-      environment {
-        EXECUTE_FILE = 'image_builder/scripts/init_image.sh'
-      }
-      // TODO: Transfer apps.sh initialisation code here
-      steps {
-        sh "$WORKSPACE/image_builder/image_config.sh execute ${params.BUILD_DIR}/${params.IMAGE_NAME} $WORKSPACE/$EXECUTE_FILE ${params.IMAGE_VERSION} \$(basename ${params.RPI_DONWLOAD_URL})"
-      }
-    }
-    stage('Hardware setup') {
-      environment {
-        EXECUTE_FILE = 'image_builder/scripts/hardware_setup.sh'
-      }
-      steps {
-        sh "$WORKSPACE/image_builder/image_config.sh execute ${params.BUILD_DIR}/${params.IMAGE_NAME} $WORKSPACE/$EXECUTE_FILE"
-      }
-    }
-    stage('Software install') {
-      environment {
-        EXECUTE_FILE = 'image_builder/scripts/software_install.sh'
-      }
-      steps {
-        sh "$WORKSPACE/image_builder/image_config.sh execute ${params.BUILD_DIR}/${params.IMAGE_NAME} $WORKSPACE/$EXECUTE_FILE"
-      }
-    }
-    stage('Network setup') {
-      environment {
-        EXECUTE_FILE = 'image_builder/scripts/network_setup.sh'
-      }
-      steps {
-        sh "$WORKSPACE/image_builder/image_config.sh execute ${params.BUILD_DIR}/${params.IMAGE_NAME} $WORKSPACE/$EXECUTE_FILE"
-      }
-    }
-    stage('Install ROS') {
-      environment {
-        EXECUTE_FILE = 'image_builder/scripts/ros_install.sh'
-        MOVE_FILE = 'image_builder/kinetic-ros-coex.rosinstall'
-        MOVE_TO = '/home/pi/ros_catkin_ws'
-      }
-      steps {
-        sh "if ! ${params.DISCOVER_ROS_PACKAGES}; then $WORKSPACE/image_builder/image_config.sh copy_to_chroot ${params.BUILD_DIR}/${params.IMAGE_NAME} $WORKSPACE/$MOVE_FILE $MOVE_TO; fi"
-        sh "$WORKSPACE/image_builder/image_config.sh execute ${params.BUILD_DIR}/${params.IMAGE_NAME} $WORKSPACE/$EXECUTE_FILE ${params.GWBT_URL} ${params.GWBT_REF} ${params.DISCOVER_ROS_PACKAGES}"
-      }
-    }
-    // TODO: Add finalising step, transfer mirror removal from ros.sh
-    stage('Shrink image') {
-      environment {
-        EXECUTE_FILE = 'image_builder/scripts/change_boot_part.sh'
-      }
-      when { expression { return params.SHRINK } }
-      steps {
-        sh "$WORKSPACE/image_builder/autosizer.sh ${params.BUILD_DIR}/${params.IMAGE_NAME}"
-        sh "$WORKSPACE/image_builder/image_config.sh execute ${params.BUILD_DIR}/${params.IMAGE_NAME} $WORKSPACE/$EXECUTE_FILE"
-      }
-    }
-  }
-}

image_builder/image_config.sh

@@ -1,385 +0,0 @@
-#!/bin/bash
-
-# Exit immidiately on non-zero result
-set -e
-
-#
-# Script for image configure
-# @urpylka Artem Smirnov
-# @dvornikov-aa Andrey Dvornikov
-#
-
-get_image() {
-
-# STATIC FUNCTION
-# TEMPLATE: get_image $BUILD_DIR $RPI_DONWLOAD_URL $IMAGE_NAME
-
-  local RPI_ZIP_NAME=$(basename $2)
-  if [ ! -e "$1/${RPI_ZIP_NAME}" ];
-  then
-    echo "$(date) | 1. Downloading original Linux distribution"
-    wget -nv -O $1/${RPI_ZIP_NAME} $2
-    echo "$(date) | Downloading complete"
-  else
-    echo "$(date) | 1. Linux distribution already donwloaded"
-  fi
-  echo "$(date) | 2. Unzipping Linux distribution image"
-  local RPI_IMAGE_NAME=$(echo ${RPI_ZIP_NAME} | sed 's/zip/img/')
-  unzip -p $1/${RPI_ZIP_NAME} ${RPI_IMAGE_NAME} > $1/$3
-  echo "$(date) | Unzipping complete"
-}
-
-resize_fs() {
-
-  # STATIC FUNCTION
-  # TEMPLATE: resize_fs $IMAGE_PATH $SIZE
-
-  # Partitions numbers
-  local BOOT_PARTITION=1
-  local ROOT_PARTITION=2
-
-  set +e
-
-  # https://ru.wikipedia.org/wiki/%D0%A0%D0%B0%D0%B7%D1%80%D0%B5%D0%B6%D1%91%D0%BD%D0%BD%D1%8B%D0%B9_%D1%84%D0%B0%D0%B9%D0%BB
-
-  # https://raspberrypi.stackexchange.com/questions/13137/how-can-i-mount-a-raspberry-pi-linux-distro-image
-  # fdisk -l 2017-11-29-raspbian-stretch-lite.img
-  # https://www.stableit.ru/2011/05/losetup.html
-  # -f     : losetup сам выбрал loop (минуя занятые)
-  # -P     : losetup монтирует разделы в образе как отдельные подразделы,
-  #          например /dev/loop0p1 и /dev/loop0p2
-  # --show : печатает имя устройства, например /dev/loop4
-
-  # http://karelzak.blogspot.ru/2015/05/resize-by-sfdisk.html
-  # ", +" : expand partition for volume size
-  # -N 2  : select second partition for work
-
-  # There is a risk that sfdisk will ask for a disk remount to update partition table
-  # TODO: Check sfdisk exit code
-
-  echo -e "\033[0;31m\033[1mTruncate image\033[0m\033[0m" \
-    && truncate -s$2 $1 \
-    && echo "Mount loop-image: $1" \
-    && local DEV_IMAGE=$(losetup -Pf $1 --show) \
-    && sleep 0.5 \
-    && echo -e "\033[0;31m\033[1mMount loop-image: $1\033[0m\033[0m" \
-    && echo ", +" | sfdisk -N ${ROOT_PARTITION} ${DEV_IMAGE} \
-    && sleep 0.5 \
-    && losetup -d ${DEV_IMAGE} \
-    && sleep 0.5 \
-    && local DEV_IMAGE=$(losetup -Pf $1 --show) \
-    && sleep 0.5 \
-    && echo -e "\033[0;31m\033[1mCheck & repair filesystem after expand partition\033[0m\033[0m" \
-    && e2fsck -fvy "${DEV_IMAGE}p${ROOT_PARTITION}" \
-    && echo -e "\033[0;31m\033[1mExpand filesystem\033[0m\033[0m" \
-    && resize2fs "${DEV_IMAGE}p${ROOT_PARTITION}" \
-    && echo -e "\033[0;31m\033[1mUmount loop-image\033[0m\033[0m" \
-    && losetup -d ${DEV_IMAGE}
-
-  set -e
-}
-
-mount_system() {
-
-  # STATIC FUNCTION
-  # TEMPLATE: mount_system $IMAGE
-
-  # Partitions numbers
-  local BOOT_PARTITION=1
-  local ROOT_PARTITION=2
-
-  # https://www.stableit.ru/2011/05/losetup.html
-  # -f     : losetup выбирает незанятое имя устройства, например /dev/loop2
-  # -P     : losetup монтирует разделы в образе как отдельные подразделы,
-  #          например /dev/loop0p1 и /dev/loop0p2
-  # --show : печатает имя устройства, например /dev/loop4
-
-  echo -e "\033[0;31m\033[1mMount loop-image: $1\033[0m\033[0m"
-  local DEV_IMAGE=$(losetup -Pf $1 --show)
-  sleep 0.5
-
-  # Get temp directory to mount image
-  local MOUNT_POINT=$(mktemp -d)
-
-  echo -e "\033[0;31m\033[1mMount dirs ${MOUNT_POINT} & ${MOUNT_POINT}/boot\033[0m\033[0m"
-  mount "${DEV_IMAGE}p${ROOT_PARTITION}" ${MOUNT_POINT}
-  mount "${DEV_IMAGE}p${BOOT_PARTITION}" ${MOUNT_POINT}/boot
-
-  echo -e "\033[0;31m\033[1mBind system dirs\033[0m\033[0m"
-  # https://github.com/debian-pi/raspbian-ua-netinst/issues/314
-  echo "Mounting /proc in chroot... "
-  if [ ! -d ${MOUNT_POINT}/proc ] ; then
-    mkdir -p ${MOUNT_POINT}/proc \
-      && echo "Created ${MOUNT_POINT}/proc"
-  fi
-  mount -t proc -o nosuid,noexec,nodev proc ${MOUNT_POINT}/proc \
-    && echo "OK"
-  
-  echo "Mounting /sys in chroot... "
-  if [ ! -d ${MOUNT_POINT}/sys ] ; then
-    mkdir -p ${MOUNT_POINT}/sys \
-      && echo "Created ${MOUNT_POINT}/sys"
-  fi
-  mount -t sysfs -o nosuid,noexec,nodev sysfs ${MOUNT_POINT}/sys \
-    && echo "OK"
-  
-  echo "Mounting /dev/ and /dev/pts in chroot... " \
-    && mkdir -p -m 755 ${MOUNT_POINT}/dev/pts \
-    && mount -t devtmpfs -o mode=0755,nosuid devtmpfs ${MOUNT_POINT}/dev \
-    && mount -t devpts -o gid=5,mode=620 devpts ${MOUNT_POINT}/dev/pts \
-    && echo "OK"
-  # mount -t devpts none "${MOUNT_POINT}/dev/pts" -o ptmxmode=0666,newinstance
-  # ln -fs "pts/ptmx" "${MOUNT_POINT}/dev/ptmx"
-
-  # mount -o bind /dev ${MOUNT_POINT}/dev
-  # mount -t proc proc ${MOUNT_POINT}/proc
-  # mount -t devpts devpts ${MOUNT_POINT}/dev/pts
-
-  # mount -t proc proc ${MOUNT_POINT}/proc
-  # mount -t sysfs sys ${MOUNT_POINT}/sys
-  # mount --bind /dev ${MOUNT_POINT}/dev
-
-  echo -e "\033[0;31m\033[1mCopy DNS records\033[0m\033[0m" \
-    && cp -L /etc/resolv.conf ${MOUNT_POINT}/etc/resolv.conf
-
-  # https://wiki.archlinux.org/index.php/Change_root_(%D0%A0%D1%83%D1%81%D1%81%D0%BA%D0%B8%D0%B9)
-  # http://www.unix-lab.org/posts/chroot/
-  # https://habrahabr.ru/post/141012/
-  # https://losst.ru/vosstanovlenie-grub2
-  # http://unixteam.ru/content/virtualizaciya-ili-zapuskaem-prilozhenie-v-chroot-okruzhenii-razmyshleniya
-  # http://help.ubuntu.ru/wiki/%D0%B2%D0%BE%D1%81%D1%81%D1%82%D0%B0%D0%BD%D0%BE%D0%B2%D0%BB%D0%B5%D0%BD%D0%B8%D0%B5_grub
-  echo -e "\033[0;31m\033[1mEnter chroot\033[0m\033[0m" \
-    && chroot ${MOUNT_POINT} /bin/bash
-
-  umount_system ${MOUNT_POINT} ${DEV_IMAGE}
-}
-
-execute() {
-
-  # STATIC FUNCTION
-  # TEMPLATE: execute $IMAGE $EXECUTE_FILE ...
-
-  # Partitions numbers
-  local BOOT_PARTITION=1
-  local ROOT_PARTITION=2
-
-  echo -e "\033[0;31m\033[1mMount loop-image: $1\033[0m\033[0m"
-  local DEV_IMAGE=$(losetup -Pf $1 --show)
-  sleep 0.5
-
-  # Get temp directory to mount image
-  local MOUNT_POINT=$(mktemp -d)
-
-  echo -e "\033[0;31m\033[1mMount dirs ${MOUNT_POINT} & ${MOUNT_POINT}/boot\033[0m\033[0m"
-  mount "${DEV_IMAGE}p${ROOT_PARTITION}" ${MOUNT_POINT}
-  mount "${DEV_IMAGE}p${BOOT_PARTITION}" ${MOUNT_POINT}/boot
-
-  echo -e "\033[0;31m\033[1mBind system dirs\033[0m\033[0m"
-  echo "Mounting /proc in chroot... "
-  if [ ! -d ${MOUNT_POINT}/proc ] ; then
-    mkdir -p ${MOUNT_POINT}/proc
-    echo "Created ${MOUNT_POINT}/proc"
-  fi
-  mount -t proc -o nosuid,noexec,nodev proc ${MOUNT_POINT}/proc \
-    && echo "OK"
-
-  echo "Mounting /sys in chroot... "
-  if [ ! -d ${MOUNT_POINT}/sys ] ; then
-    mkdir -p ${MOUNT_POINT}/sys
-    echo "Created ${MOUNT_POINT}/sys"
-  fi
-  mount -t sysfs -o nosuid,noexec,nodev sysfs ${MOUNT_POINT}/sys \
-    && echo "OK"
-
-  echo "Mounting /dev/ and /dev/pts in chroot... " \
-    && mkdir -p -m 755 ${MOUNT_POINT}/dev/pts \
-    && mount -t devtmpfs -o mode=0755,nosuid devtmpfs ${MOUNT_POINT}/dev \
-    && mount -t devpts -o gid=5,mode=620 devpts ${MOUNT_POINT}/dev/pts \
-    && echo "OK"
-
-  echo -e "\033[0;31m\033[1mCopy DNS records\033[0m\033[0m" \
-    && cp -L /etc/resolv.conf ${MOUNT_POINT}/etc/resolv.conf
-
-  echo -e "\033[0;31m\033[1m$(date) | Enter chroot\033[0m\033[0m"
-  local script_name=$(basename $2)
-  local script_path_root="${MOUNT_POINT}/root/${script_name}"
-  # Copy script into chroot fs
-  # TODO: Find more suitable location for temporary script storage
-  cp "$2" "${script_path_root}"
-  # Its important to save arguments (direct ${@:4} causes problems)
-  script_args="${@:3}"
-  # Run script in chroot with additional arguments
-  chroot ${MOUNT_POINT} /bin/sh -c "/root/${script_name} ${script_args}"
-  # Removing script from chroot fs
-  rm "${script_path_root}"
-
-  umount_system ${MOUNT_POINT} ${DEV_IMAGE}
-}
-
-copy_to_chroot() {
-
-  # STATIC FUNCTION
-  # TEMPLATE: copy_to_chroot $IMAGE $MOVE_FILE $MOVE_TO
-
-  # Partitions numbers
-  local BOOT_PARTITION=1
-  local ROOT_PARTITION=2
-
-  echo -e "\033[0;31m\033[1mMount loop-image: $1\033[0m\033[0m"
-  local DEV_IMAGE=$(losetup -Pf $1 --show)
-  sleep 0.5
-
-  # Get temp directory to mount image
-  local MOUNT_POINT=$(mktemp -d)
-
-  echo -e "\033[0;31m\033[1mMount dirs ${MOUNT_POINT} & ${MOUNT_POINT}/boot\033[0m\033[0m"
-  mount "${DEV_IMAGE}p${ROOT_PARTITION}" ${MOUNT_POINT}
-  mount "${DEV_IMAGE}p${BOOT_PARTITION}" ${MOUNT_POINT}/boot
-
-  echo -e "\033[0;31m\033[1m$(date) | Enter chroot\033[0m\033[0m"
-  file_name=$(basename $2)
-  file_path_root="${MOUNT_POINT}$3/${file_name}"
-  # Copy script into chroot fs
-  # TODO: Find more suitable location for temporary script storage
-  if [ ! -d ${file_path_root} ] ; then
-    mkdir -p ${file_path_root} \
-      && echo "Created ${file_path_root}"
-  fi
-  cp "$2" "${file_path_root}"
-
-  umount_system ${MOUNT_POINT} ${DEV_IMAGE}
-}
-
-umount_system() {
-
-  # STATIC FUNCTION
-  # TEMPLATE: umount_system $MOUNT_POINT $DEV_IMAGE
-
-  echo -e "\033[0;31m\033[1m$(date) | Umount recursive dirs: $1\033[0m\033[0m"
-  # There is a risk that umount will fail
-  set +e
-  # Successfull unmount flag (false at thismoment)
-  umount_ok=false
-  # Repeat 5 times
-  for i in {1..5}
-  do
-    # Unmount chroot rootfs and boot partition
-    umount -fR $1
-    # If no problems detected
-    if [[ $? == 0 ]]
-    then
-    echo -e "\033[0;31m\033[1m$(date) | Successfull unmount\033[0m\033[0m"
-    # Set flag
-    umount_ok=true
-    # Exit loop
-    break
-    fi
-    # Unmount has failed
-    echo -e "\033[0;31m\033[1m$(date) | Unmount failed\033[0m\033[0m"
-    # Wait for some time
-    sleep 2
-  done
-  set -e
-  # Jenkins job will fail if this condition is not true
-  [[ "$umount_ok" == true ]]
-  echo -e "\033[0;31m\033[1m$(date) | Umount loop-image\033[0m\033[0m"
-  #losetup -d $DEV_IMAGE
-  losetup -d $2
-}
-
-publish_image() {
-
-# STATIC FUNCTION
-# TEMPLATE: publish_image_bash $BUILD_DIR $IMAGE_NAME $YA_SCRIPT $CONFIG_FILE $RELEASE_ID $RELEASE_BODY
-
-# https://developer.github.com/v3/repos/releases/
-#RELEASE_BODY="### Changelog\n* Add /boot/cmdline.txt net.ifnames=0 https://www.freedesktop.org/wiki/Software/systemd/PredictableNetworkInterfaceNames/\n* Updated cophelper\n* Installed copstat"
-
-  echo -e "\033[0;31m\033[1m$(date) | Zip image\033[0m\033[0m"
-  if [ ! -e "$1/$2.zip" ];
-  then
-    cd $1 && zip $2.zip $2
-    echo -e "\033[0;31m\033[1m$(date) | Zipping complete!\033[0m\033[0m"
-  else
-    echo -e "\033[0;31m\033[1m$(date) | Zip-archive already created\033[0m\033[0m"
-    cd $1 && rm $2.zip && zip $2.zip $2
-    echo -e "\033[0;31m\033[1m$(date) | Old archive was deleted & create new\033[0m\033[0m"
-  fi
-
-  echo -e "\033[0;31m\033[1m$(date) | Upload image\033[0m\033[0m"
-  local IMAGE_LINK=$($3 $4 $1/$2.zip)
-  echo -e "\033[0;31m\033[1m$(date) | Upload copmlete!\033[0m\033[0m"
-
-  echo -e "\033[0;31m\033[1m$(date) | Meashure size of zip-image\033[0m\033[0m"
-  local IMAGE_SIZE=$(du -sh $1/$2.zip | awk '{ print $1 }')
-  echo -e "\033[0;31m\033[1m$(date) | Meashuring copmlete!\033[0m\033[0m"
-
-  echo -e "\033[0;31m\033[1m$(date) | Meashure hash-sum of zip-image\033[0m\033[0m"
-  local IMAGE_HASH=$(sha256sum $1/$2.zip | awk '{ print $1 }')
-  echo -e "\033[0;31m\033[1m$(date) | Meashuring copmlete!\033[0m\033[0m"
-
-  echo ""
-  echo "\$6: $6"
-  echo ""
-
-  echo -e "\033[0;31m\033[1m$(date) | Post message to GH\033[0m\033[0m"
-  local NEW_RELEASE_BODY="### Download\n* [$2.zip]($IMAGE_LINK) ($IMAGE_SIZE)\nsha256: $IMAGE_HASH\n\n$6"
-  local DATA="{ \"body\":\"$NEW_RELEASE_BODY\" }"
-
-  echo ""
-  echo "\$DATA: $DATA"
-  echo ""
-
-  local GH_LOGIN=$(cat $4 | jq '.github.login' -r)
-  local GH_PASS=$(cat $4 | jq '.github.password' -r)
-  local GH_URL=$(cat $4 | jq '.github.url' -r)
-  curl -d "$DATA" -u "$GH_LOGIN:$GH_PASS" --request PATCH $GH_URL$5
-  echo -e "\033[0;31m\033[1m$(date) | Post message to GH copmlete!\033[0m\033[0m"
-}
-
-if [ $(whoami) != "root" ];
-then echo "" \
-  && echo "********************************************************************" \
-  && echo "******************** This should be run as root ********************" \
-  && echo "********************************************************************" \
-  && echo "" \
-  && exit 1
-fi
-
-echo "\$#: $#"
-echo "\$1: $1"
-echo "\$2: $2"
-echo "\$3: $3"
-echo "\$4: $4"
-echo "\$5: $5"
-echo "\$6: $6"
-echo "\$7: $7"
-
-case "$1" in
-  mount_system)
-  # mount_system $IMAGE
-    mount_system $2;;
-
-  get_image)
-  # get_image $BUILD_DIR $RPI_DONWLOAD_URL $IMAGE_NAME
-    get_image $2 $3 $4;;
-
-  resize_fs)
-  # resize_fs $IMAGE_PATH $SIZE
-    resize_fs $2 $3;;
-
-  publish_image)
-  # publish_image $BUILD_DIR $IMAGE_NAME $YA_SCRIPT $CONFIG_FILE $RELEASE_ID $RELEASE_BODY
-    publish_image $2 $3 $4 $5 $6 "$7";;
-
-  execute)
-  # execute $IMAGE $EXECUTE_FILE ...
-    execute $2 $3 ${@:4};;
-
-  copy_to_chroot)
-  # copy_to_chroot $IMAGE $MOVE_FILE $MOVE_TO
-    copy_to_chroot $2 $3 $4;;
-
-  *)
-    echo "Enter one of: mount_system, get_image, resize_fs, publish_image, execute";;
-esac

image_builder/scripts/change_boot_part.sh

@@ -1,11 +0,0 @@
-#!/bin/bash
-
-set -e
-
-echo -e "\033[0;31m\033[1m$(date) | #1 Change boot partition\033[0m\033[0m"
-
-sed -i 's/root=[^ ]*/root=\/dev\/mmcblk0p2/' /boot/cmdline.txt
-sed -i 's/.*  \/boot           vfat    defaults          0       2$/\/dev\/mmcblk0p1  \/boot           vfat    defaults          0       2/' /etc/fstab
-sed -i 's/.*  \/               ext4    defaults,noatime  0       1$/\/dev\/mmcblk0p2  \/               ext4    defaults,noatime  0       1/' /etc/fstab
-
-echo -e "\033[0;31m\033[1m$(date) | End of change boot partition\033[0m\033[0m"

image_builder/scripts/init_image.sh

@@ -1,38 +0,0 @@
-#!/bin/bash
-
-set -e
-
-##################################################################################################################################
-# Image initialisation
-##################################################################################################################################
-
-# Add apt key to allow local mirror usage during image build
-#wget -O - ftp://192.168.0.10/coex-mirror.gpg | apt-key add -
-# Generate a backup of the original source.list
-#cp /etc/apt/sources.list /var/sources.list.bak
-# Add the local mirror as the first priority repository
-#wget -O - ftp://192.168.0.10/coex-mirror.list 2>/dev/null | cat - /etc/apt/sources.list > /var/sources.list && mv /var/sources.list /etc/apt/sources.list
-
-echo -e "\033[0;31m\033[1m$(date) | #1 apt cache update\033[0m\033[0m"
-
-# Clean repostory cache
-apt-get clean
-# Update repository cache
-apt-get update
-# && apt upgrade -y
-
-echo -e "\033[0;31m\033[1m$(date) | #2 Write clever information\033[0m\033[0m"
-
-# Clever image version
-echo "$1" >> /etc/clever_version
-# Origin image file name
-echo "${2%.*}" >> /etc/clever_origin
-
-
-echo -e "\033[0;31m\033[1m$(date) | #3 Set max space for syslogs\033[0m\033[0m"
-
-# https://unix.stackexchange.com/questions/139513/how-to-clear-journalctl
-sed -i 's/#SystemMaxUse=/SystemMaxUse=200M/' /etc/systemd/journald.conf
-
-
-echo -e "\033[0;31m\033[1m$(date) | #4 End initialisation of image\033[0m\033[0m"

image_builder/scripts/network_setup.sh

@@ -1,45 +0,0 @@
-#!/bin/bash
-
-set -e
-
-echo -e "\033[0;31m\033[1m$(date) | #1 Write to /etc/wpa_supplicant/wpa_supplicant.conf\033[0m\033[0m"
-
-# TODO: Use wpa_cli insted direct file edit
-echo "
-network={
-    ssid=\"CLEVER\"
-    psk=\"cleverwifi\"
-    mode=2
-    proto=RSN
-    key_mgmt=WPA-PSK
-    pairwise=CCMP
-    group=CCMP
-    auth_alg=OPEN
-}" >> /etc/wpa_supplicant/wpa_supplicant.conf
-
-echo -e "\033[0;31m\033[1m$(date) | #2 Write STATIC to /etc/dhcpcd.conf\033[0m\033[0m"
-
-echo "
-interface wlan0
-static ip_address=192.168.11.1/24" >> /etc/dhcpcd.conf
-
-echo -e "\033[0;31m\033[1m$(date) | #3 Write dhcp-config to /etc/dnsmasq.conf\033[0m\033[0m"
-
-echo "
-interface=wlan0
-address=/clever/coex/192.168.11.1
-dhcp-range=192.168.11.100,192.168.11.200,12h
-no-hosts
-filterwin2k
-bogus-priv
-domain-needed
-quiet-dhcp6
-" >> /etc/dnsmasq.conf
-
-echo -e "\033[0;31m\033[1m$(date) | #4 Write magic script for rename SSID to /etc/rc.local\033[0m\033[0m"
-
-RENAME_SSID="sudo sed -i.OLD \"s/CLEVER/CLEVER-\$(head -c 100 /dev/urandom | xxd -ps -c 100 | sed -e 's/[^0-9]//g' | cut -c 1-4)/g\" /etc/wpa_supplicant/wpa_supplicant.conf && sudo sed -i '/sudo sed/d' /etc/rc.local && sudo reboot"
-
-sed -i "19a$RENAME_SSID" /etc/rc.local
-
-echo -e "\033[0;31m\033[1m$(date) | #5 End of network installation\033[0m\033[0m"

image_builder/scripts/ros_install.sh

@@ -1,168 +0,0 @@
-#!/bin/bash
-
-set -e
-
-##################################################################################################################################
-# ROS for user pi
-##################################################################################################################################
-
-# ros http://wiki.ros.org/action/fullsearch/ROSberryPi/Installing%20ROS%20Kinetic%20on%20the%20Raspberry%20Pi
-
-echo -e "\033[0;31m\033[1m$(date) | Installing ROS\033[0m\033[0m"
-
-echo -e "\033[0;31m\033[1m$(date) | #1 Installing dirmngr & add key to apt-key\033[0m\033[0m"
-
-# Install a tool that apt-key uses to add ROS repository key
-# http://wpblogger.su/tags/apt/
-apt-get install --no-install-recommends -y dirmngr=2.1.18-8~deb9u2
-# setup keys
-apt-key adv --keyserver hkp://ha.pool.sks-keyservers.net:80 --recv-key 421C365BD9FF1F717815A3895523BAEEB01FA116
-
-# setup sources.list
-echo "deb http://packages.ros.org/ros/ubuntu stretch main" > /etc/apt/sources.list.d/ros-latest.list
-
-echo -e "\033[0;31m\033[1m$(date) | #2 apt update && apt upgrade\033[0m\033[0m"
-
-# install bootstrap tools
-apt-get update
-# && apt upgrade -y
-
-echo -e "\033[0;31m\033[1m$(date) | #3 Installing wget, unzip, python-rosdep, python-rosinstall-generator, python-wstool, python-rosinstall, build-essential, cmake\033[0m\033[0m"
-
-apt-get install --no-install-recommends -y \
-  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
-
-echo -e "\033[0;31m\033[1m$(date) | #4 rosdep init && rosdep update\033[0m\033[0m"
-
-# bootstrap rosdep
-rosdep init && rosdep update
-
-# If $3 = false, then discover packages
-if [ "$3" = "false" ];
-then
-  echo -e "\033[0;31m\033[1m$(date) | #5 Preparing ros_comm packages to kinetic-ros_comm-wet.rosinstall\033[0m\033[0m"
-
-  # create ros catkin workspace
-  mkdir -p /home/pi/ros_catkin_ws && cd /home/pi/ros_catkin_ws \
-      && rosinstall_generator ros_comm --rosdistro kinetic --deps --wet-only --tar > kinetic-ros_comm-wet.rosinstall \
-      && wstool init src kinetic-ros_comm-wet.rosinstall
-
-  echo -e "\033[0;31m\033[1m$(date) | #6 Preparing other ROS-packages to kinetic-custom_ros.rosinstall\033[0m\033[0m"
-
-  cd /home/pi/ros_catkin_ws \
-    && rosinstall_generator \
-    actionlib actionlib_msgs angles async_web_server_cpp bond bond_core bondcpp bondpy camera_calibration_parsers camera_info_manager catkin class_loader cmake_modules cpp_common cv_bridge cv_camera diagnostic_msgs diagnostic_updater dynamic_reconfigure eigen_conversions gencpp geneus genlisp genmsg gennodejs genpy geographic_msgs geometry_msgs geometry2 image_transport compressed_image_transport libmavconn mavlink mavros_msgs message_filters message_generation message_runtime mk nav_msgs nodelet orocos_kdl pluginlib python_orocos_kdl ros ros_comm rosapi rosauth rosbag rosbag_migration_rule rosbag_storage rosbash rosboost_cfg rosbridge_library rosbridge_server rosbridge_suite rosbuild rosclean rosconsole rosconsole_bridge roscpp roscpp_serialization roscpp_traits roscreate rosgraph rosgraph_msgs roslang roslaunch roslib roslint roslisp roslz4 rosmake rosmaster rosmsg rosnode rosout rospack rosparam rospy rospy_tutorials rosserial rosserial_client rosserial_msgs rosserial_python rosservice rostest rostime rostopic rosunit roswtf sensor_msgs smclib std_msgs std_srvs stereo_msgs tf tf2 tf2_bullet tf2_eigen tf2_geometry_msgs tf2_kdl tf2_msgs tf2_py tf2_ros tf2_sensor_msgs tf2_tools topic_tools trajectory_msgs urdf urdf_parser_plugin usb_cam uuid_msgs visualization_msgs web_video_server xmlrpcpp mavros opencv3 mavros_extras interactive_markers tf2_web_republisher interactive_marker_proxy \
-    --rosdistro kinetic --deps --wet-only --tar > kinetic-custom_ros.rosinstall \
-    && wstool merge -t src kinetic-custom_ros.rosinstall \
-    && wstool update -t src
-else
-  echo -e "\033[0;31m\033[1m$(date) | #5 Creating manual ros_catkin_ws\033[0m\033[0m"
-
-  mkdir -p /home/pi/ros_catkin_ws && cd /home/pi/ros_catkin_ws \
-    && wstool init src kinetic-ros-coex.rosinstall
-fi
-
-echo -e "\033[0;31m\033[1m$(date) | #7 Installing dependencies apps with rosdep\033[0m\033[0m"
-cd /home/pi/ros_catkin_ws
-# There is a risk that umount will fail
-set +e
-# Successfull unmount flag (false at thismoment)
-install_ok=false
-# Repeat 5 times
-for i in {1..5}
-do
-  # Resolving Dependencies with rosdep
-  rosdep install -y --from-paths src --ignore-src --rosdistro kinetic -r --os=debian:stretch
-  # If no problems detected
-  if [[ $? == 0 ]]
-  then
-    echo -e "\033[0;31m\033[1m$(date) | Successfull rosdep install\033[0m\033[0m"
-    # Set flag
-    install_ok=true
-    # Exit loop
-    break
-  fi
-  # Unmount has failed
-  echo -e "\033[0;31m\033[1m$(date) | Rosdep installation failed\033[0m\033[0m"
-  # Wait for some time
-  sleep 2
-done
-set -e
-# Jenkins job will fail if this condition is not true
-[[ "$install_ok" == true ]]
-echo -e "\033[0;31m\033[1m$(date) | End of rosdep install\033[0m\033[0m"
-
-echo -e "\033[0;31m\033[1m$(date) | #8 Refactoring usb_cam in SRC\033[0m\033[0m"
-
-sed -i '/#define __STDC_CONSTANT_MACROS/a\#define PIX_FMT_RGB24 AV_PIX_FMT_RGB24\n#define PIX_FMT_YUV422P AV_PIX_FMT_YUV422P' /home/pi/ros_catkin_ws/src/usb_cam/src/usb_cam.cpp
-
-echo -e "\033[0;31m\033[1m$(date) | #9 Installing GeographicLib datasets\033[0m\033[0m"
-
-/home/pi/ros_catkin_ws/src/mavros/mavros/scripts/install_geographiclib_datasets.sh
-
-#echo -e "\033[0;31m\033[1m$(date) | #11 Building light packages on 2 threads\033[0m\033[0m"
-
-# Build the catkin Workspace
-#cd /home/pi/ros_catkin_ws && ./src/catkin/bin/catkin_make_isolated --install -DCMAKE_BUILD_TYPE=Release -j2 --install-space /opt/ros/kinetic --pkg actionlib actionlib_msgs angles async_web_server_cpp bond bond_core bondcpp bondpy camera_calibration_parsers camera_info_manager catkin class_loader cmake_modules cpp_common diagnostic_msgs diagnostic_updater dynamic_reconfigure eigen_conversions gencpp geneus genlisp genmsg gennodejs genpy geographic_msgs geometry_msgs geometry2 image_transport libmavconn mavlink mavros_msgs message_filters message_generation message_runtime mk nav_msgs nodelet orocos_kdl pluginlib python_orocos_kdl ros ros_comm rosapi rosauth rosbag rosbag_migration_rule rosbag_storage rosbash rosboost_cfg rosbridge_library rosbridge_server rosbridge_suite rosbuild rosclean rosconsole rosconsole_bridge roscpp roscpp_serialization roscpp_traits roscreate rosgraph rosgraph_msgs roslang roslaunch roslib roslint roslisp roslz4 rosmake rosmaster rosmsg rosnode rosout rospack rosparam rospy rospy_tutorials rosserial rosserial_client rosserial_msgs rosserial_python rosservice rostest rostime rostopic rosunit roswtf sensor_msgs smclib std_msgs std_srvs stereo_msgs tf tf2 tf2_bullet tf2_eigen tf2_geometry_msgs tf2_kdl tf2_msgs tf2_py tf2_ros tf2_sensor_msgs tf2_tools topic_tools trajectory_msgs urdf urdf_parser_plugin usb_cam uuid_msgs visualization_msgs xmlrpcpp
-
-#echo -e "\033[0;31m\033[1m$(date) | #12 Building heavy packages\033[0m\033[0m"
-
-# This command uses less threads to avoid Raspberry Pi freeze
-# Build the catkin Workspace
-#cd /home/pi/ros_catkin_ws && ./src/catkin/bin/catkin_make_isolated --install -DCMAKE_BUILD_TYPE=Release -j1 --install-space /opt/ros/kinetic --pkg mavros opencv3 cv_bridge cv_camera mavros_extras web_video_server
-
-echo -e "\033[0;31m\033[1m$(date) | #10 Building packages on 1 thread\033[0m\033[0m"
-
-# Install builded packages
-# WARNING: A major bug was found when using --pkg option (catkin_make_isolated doesn't install environment files)
-# TODO: Can we increase threads number with HDD swap?
-cd /home/pi/ros_catkin_ws && ./src/catkin/bin/catkin_make_isolated --install -j1 -DCMAKE_BUILD_TYPE=Release --install-space /opt/ros/kinetic
-
-echo -e "\033[0;31m\033[1m$(date) | #11 Remove build_isolated & devel_isolated from ros_catkin_ws\033[0m\033[0m"
-
-rm -rf /home/pi/ros_catkin_ws/build_isolated /home/pi/ros_catkin_ws/devel_isolated
-chown -Rf pi:pi /home/pi/ros_catkin_ws
-
-echo -e "\033[0;31m\033[1m$(date) | #12 Creating catkin_ws & Installing CLEVER-BUNDLE\033[0m\033[0m"
-
-git clone $1 /home/pi/catkin_ws/src/clever \
-  && cd /home/pi/catkin_ws/src/clever \
-  && git checkout $2 \
-  && pip install wheel \
-  && pip install -r /home/pi/catkin_ws/src/clever/clever/requirements.txt \
-  && cd /home/pi/catkin_ws \
-  && . /opt/ros/kinetic/setup.sh \
-  && catkin_make -j1 -DCMAKE_BUILD_TYPE=Release \
-  && ln -s /home/pi/catkin_ws/src/clever/deploy/roscore.service /lib/systemd/system/roscore.service \
-  && ln -s /home/pi/catkin_ws/src/clever/deploy/clever.service /lib/systemd/system/clever.service \
-  && systemctl enable roscore \
-  && systemctl enable clever
-
-echo -e "\033[0;31m\033[1m$(date) | #13 Change permissions for catkin_ws\033[0m\033[0m"
-
-chown -Rf pi:pi /home/pi/catkin_ws
-
-echo -e "\033[0;31m\033[1m$(date) | #14 Setup ROS environment\033[0m\033[0m"
-
-cat <<EOF | tee -a /home/pi/.bashrc > /dev/null
-LANG=C.UTF-8
-LC_ALL=C.UTF-8
-ROS_DISTRO=kinetic
-export ROS_IP=192.168.11.1
-source /opt/ros/kinetic/setup.bash
-source /home/pi/catkin_ws/devel/setup.bash
-EOF
-
-#echo -e "\033[0;31m\033[1m$(date) | #14 Removing local apt mirror\033[0m\033[0m"
-# Restore original sources.list
-#mv /var/sources.list.bak /etc/apt/sources.list
-# Clean apt cache
-apt-get clean
-# Remove local mirror repository key
-#apt-key del COEX-MIRROR
-
-echo -e "\033[0;31m\033[1m$(date) | END of ROS INSTALLATION\033[0m\033[0m"

image_builder/scripts/software_install.sh

@@ -1,74 +0,0 @@
-#!/bin/bash
-
-set -e
-
-##################################################################################################################################
-# Image software installation
-##################################################################################################################################
-
-echo -e "\033[0;31m\033[1m$(date) | #1 Software installing\033[0m\033[0m"
-
-# TODO: Use dnsmasq instead of isc-dhcp-server
-apt-get install --no-install-recommends -y \
-  unzip=6.0-21 \
-  zip=3.0-11 \
-  ipython=5.1.0-3 \
-  ipython3=5.1.0-3 \
-  screen=4.5.0-6 \
-  byobu=5.112-1  \
-  nmap=7.40-1 \
-  lsof=4.89+dfsg-0.1 \
-  git=1:2.11.0-3+deb9u3 \
-  dnsmasq=2.76-5+rpt1+deb9u1  \
-  tmux=2.3-4 \
-  vim=2:8.0.0197-4+deb9u1 \
-  cmake=3.7.2-1 \
-  python-pip=9.0.1-2+rpt2 \
-  python3-pip=9.0.1-2+rpt2 \
-  libjpeg8-dev=8d1-2 \
-  tcpdump \
-  ltrace \
-  libpoco-dev=1.7.6+dfsg1-5+deb9u1
-
-# Deny byobu to check available updates
-# sudo sed -i "s/updates_available//" /usr/share/byobu/status/status
-sudo sed -i "s/updates_available//" /home/pi/.byobu/status
-
-# install Monkey web-server
-cd /home/pi
-git clone https://github.com/monkey/monkey.git
-cd monkey
-git checkout v1.6.9
-./configure --malloc-libc --local
-make
-sudo setcap 'cap_net_bind_service=+ep' ./build/monkey  # allow using 80 port
-rm build/conf/sites/default
-ln -s /home/pi/catkin_ws/src/clever/deploy/monkey ./build/conf/sites/default
-cd /home/pi
-
-# install and enable Butterfly (web terminal)
-sudo apt-get install libffi-dev
-sudo pip3 install butterfly
-sudo pip3 install butterfly[systemd]
-sudo ln -s /home/pi/catkin_ws/src/clever/deploy/butterfly.service /lib/systemd/system/
-sudo ln -s /home/pi/catkin_ws/src/clever/deploy/butterfly.socket /lib/systemd/system/
-sudo systemctl enable butterfly.socket
-
-echo -e "\033[0;31m\033[1m$(date) | #2 Adding mjpg-streamer at /home/pi\033[0m\033[0m"
-# https://github.com/jacksonliam/mjpg-streamer
-
-git clone https://github.com/jacksonliam/mjpg-streamer.git /home/pi/mjpg-streamer \
-  && cd /home/pi/mjpg-streamer/mjpg-streamer-experimental \
-  && make \
-  && make install \
-  && chown -Rf pi:pi /home/pi/mjpg-streamer
-
-echo -e "\033[0;31m\033[1m$(date) | Miscellaneous\033[0m\033[0m"
-
-# vim settings
-echo "set mouse-=a
-syntax on
-autocmd BufNewFile,BufRead *.launch set syntax=xml
-" > /home/pi/.vimrc
-
-echo -e "\033[0;31m\033[1m$(date) | End of network installation\033[0m\033[0m"

image_builder/yadisk.py

@@ -1,51 +0,0 @@
-#!/usr/bin/env python
-
-#
-# Simple python uploader to YaDisk
-# Smirnov Artem @urpylka
-#
-# Use:
-# python yadisk.py login password file server_dir
-#
-
-from YaDiskClient.YaDiskClient import YaDisk
-import os.path, sys, json
-
-def upload(_login, _password, _server_dir, _file):
-    if os.path.isfile(_file):
-        disk = YaDisk(_login, _password)
-        disk.upload(_file, _server_dir + '/' + os.path.basename(_file))
-        link = disk.publish_doc(_server_dir + '/' + os.path.basename(_file))
-        print link
-    else:
-        print "Error: file-path is bad"
-        return 1
-
-def main():
-    if (len(sys.argv) == 5):
-        print "login: " + sys.argv[1]
-        print "password: " + sys.argv[2]
-        print "server_dir: " + sys.argv[3]
-        print "file: " + sys.argv[4]
-        
-        upload(sys.argv[1], sys.argv[2], sys.argv[3], sys.argv[4])
-
-    elif (len(sys.argv) == 3):
-    	# print "config: " + sys.argv[1]
-        # print "file: " + sys.argv[2]
-        
-        if os.path.isfile(sys.argv[1]) and os.path.isfile(sys.argv[2]):
-
-            with open(sys.argv[1]) as json_data:
-                d = json.load(json_data)
-                upload(d['yadisk']['login'], d['yadisk']['password'], d['yadisk']['server_dir'], sys.argv[2])
-
-        else:
-            print "Error: file-path or config-path is bad"
-            return 1
-    else:
-        print "Error: amount of args is incorrect"
-        return 1
-
-if __name__ == '__main__':
-    main()