Merge branch 'before-merge'

# Conflicts:
#	builder/assets/clever.service
#	builder/assets/hardware_setup.sh
#	builder/assets/kinetic-ros-clever.rosinstall
#	builder/assets/roscore.env
#	builder/assets/roscore.service

clever/src/camera_markers.cpp

@@ -0,0 +1,101 @@
+/*
+ * Visualization marker for camera alignment
+ * 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 <sensor_msgs/CameraInfo.h>
+#include <visualization_msgs/Marker.h>
+#include <visualization_msgs/MarkerArray.h>
+
+using namespace visualization_msgs;
+
+double markers_scale;
+std::string camera_frame;
+
+MarkerArray createMarkers() {
+	MarkerArray markers;
+
+	Marker lens;
+	lens.header.frame_id = camera_frame;
+	lens.ns = "camera_markers";
+	lens.id = 0;
+	lens.action = Marker::ADD;
+	lens.type = visualization_msgs::Marker::CYLINDER;
+	lens.frame_locked = true;
+	lens.scale.x = 0.013 * markers_scale;
+	lens.scale.y = 0.013 * markers_scale;
+	lens.scale.z = 0.015 * markers_scale;
+	lens.color.r = 0.3;
+	lens.color.g = 0.3;
+	lens.color.b = 0.3;
+	lens.color.a = 0.9;
+	lens.pose.position.z = 0.0075 * markers_scale;
+	lens.pose.orientation.w = 1;
+
+	Marker board;
+	board.header.frame_id = camera_frame;
+	board.ns = "camera_markers";
+	board.id = 1;
+	board.action = Marker::ADD;
+	board.type = Marker::CUBE;
+	board.frame_locked = true;
+	board.scale.x = 0.024 * markers_scale;
+	board.scale.y = 0.024 * markers_scale;
+	board.scale.z = 0.001 * markers_scale;
+	board.color.r = 0.0;
+	board.color.g = 0.8;
+	board.color.b = 0.0;
+	board.color.a = 0.9;
+	board.pose.orientation.w = 1;
+
+	Marker wire;
+	wire.header.frame_id = camera_frame;
+	wire.ns = "camera_markers";
+	wire.id = 2;
+	wire.action = Marker::ADD;
+	wire.type = Marker::CUBE;
+	wire.frame_locked = true;
+	wire.scale.x = 0.014 * markers_scale;
+	wire.scale.y = 0.04 * markers_scale;
+	wire.scale.z = 0.001 * markers_scale;
+	wire.color.r = 0.9;
+	wire.color.g = 0.9;
+	wire.color.b = 1.0;
+	wire.color.a = 0.8;
+	wire.pose.position.x = 0;
+	wire.pose.position.y = (0.01 + 0.02) * markers_scale;
+	wire.pose.position.z = 0.002 * markers_scale;
+	wire.pose.orientation.w = 1;
+
+	markers.markers.push_back(lens);
+	markers.markers.push_back(board);
+	markers.markers.push_back(wire);
+
+	return markers;
+}
+
+int main(int argc, char **argv)
+{
+	ros::init(argc, argv, "camera_markers", ros::init_options::AnonymousName);
+	ros::NodeHandle nh, nh_priv("~");
+
+	nh_priv.param("scale", markers_scale, 1.0);
+
+	// wait for camera info
+	auto camera_info = ros::topic::waitForMessage<sensor_msgs::CameraInfo>("camera_info", nh);
+	camera_frame = camera_info->header.frame_id;
+
+	ros::Publisher markers_pub = nh.advertise<visualization_msgs::MarkerArray>("camera_markers", 1, true);
+	markers_pub.publish(createMarkers());
+
+	ROS_INFO("Camera markers initialized");
+	ros::spin();
+}

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

@@ -0,0 +1,85 @@
+#!/usr/bin/env python
+import copy
+
+import rospy
+import tf.transformations as t
+from interactive_markers.interactive_marker_server import InteractiveMarkerServer
+from visualization_msgs.msg import Marker, InteractiveMarker, InteractiveMarkerControl, InteractiveMarkerFeedback
+from clever import srv
+
+
+def make_box(msg):
+    marker = Marker()
+
+    marker.type = Marker.CUBE
+    marker.scale.x = msg.scale * 0.3
+    marker.scale.y = msg.scale * 0.3
+    marker.scale.z = msg.scale * 0.3
+    marker.color.r = 0.5
+    marker.color.g = 0.5
+    marker.color.b = 0.5
+    marker.color.a = 1.0
+    marker.pose.orientation.w = 1
+
+    return marker
+
+
+def make_box_control(msg):
+    control = InteractiveMarkerControl()
+    control.always_visible = True
+    control.orientation.w = 1
+    control.markers.append(make_box(msg))
+    msg.controls.append(control)
+    return control
+
+
+def make_quadcopter_marker():
+    marker = InteractiveMarker()
+    marker.header.frame_id = 'fcu'
+    marker.header.stamp = rospy.get_rostime()
+    marker.scale = 1
+    marker.pose.orientation.w = 1
+
+    marker.name = 'quadcopter'
+    marker.description = 'Quadcopter'
+
+    make_box_control(marker)
+
+    control = InteractiveMarkerControl()
+    control.orientation.w = 1
+    control.orientation.x = 0
+    control.orientation.y = 1
+    control.orientation.z = 0
+    control.interaction_mode = InteractiveMarkerControl.MOVE_ROTATE
+    marker.controls.append(copy.deepcopy(control))
+    control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
+    marker.controls.append(control)
+
+    return marker
+
+
+navigate = rospy.ServiceProxy('navigate', srv.Navigate)
+
+
+def process_feedback(feedback):
+    if feedback.event_type != InteractiveMarkerFeedback.MOUSE_UP:
+        return
+
+    p = feedback.pose.position
+    o = feedback.pose.orientation
+    yaw = t.euler_from_quaternion((o.x, o.y, o.z, o.w), axes='rzyx')[0]
+    rospy.loginfo('Navigate to %s', p)
+    rospy.loginfo(navigate(x=p.x, y=p.y, z=p.z, yaw=yaw, speed=2,
+                           frame_id=feedback.header.frame_id, auto_arm=True))
+
+
+rospy.init_node('quadcopter_im')
+
+server = InteractiveMarkerServer('quadcopter_im')
+
+int_marker = make_quadcopter_marker()
+server.insert(int_marker, process_feedback)
+server.applyChanges()
+
+rospy.loginfo('Interactive quadcopter marker initialized')
+rospy.spin()

clever/src/monkey

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

clever/src/optical_flow.cpp

@@ -0,0 +1,200 @@
+/*
+ * Optical Flow node for PX4
+ * 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 <vector>
+#include <cmath>
+#include <nodelet/nodelet.h>
+#include <pluginlib/class_list_macros.h>
+#include <image_transport/image_transport.h>
+#include <cv_bridge/cv_bridge.h>
+#include <opencv2/opencv.hpp>
+#include <tf/transform_datatypes.h>
+#include <tf2/exceptions.h>
+#include <tf2/convert.h>
+#include <tf2_ros/transform_listener.h>
+#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
+#include <mavros_msgs/OpticalFlowRad.h>
+#include <sensor_msgs/Imu.h>
+#include <geometry_msgs/Vector3Stamped.h>
+#include <geometry_msgs/PointStamped.h>
+#include <geometry_msgs/TwistStamped.h>
+
+using cv::Mat;
+
+class OpticalFlow : public nodelet::Nodelet
+{
+public:
+	OpticalFlow():
+		camera_matrix_(3, 3, CV_64F),
+		dist_coeffs_(8, 1, CV_64F),
+		tf_listener_(tf_buffer_)
+	{}
+
+private:
+	ros::Publisher flow_pub_, velo_pub_, shift_pub_;
+	ros::Time prev_stamp_;
+	std::string fcu_frame_id_;
+	image_transport::CameraSubscriber img_sub_;
+	image_transport::Publisher img_pub_;
+	mavros_msgs::OpticalFlowRad flow_;
+	int roi_, roi_2_;
+	Mat hann_;
+	Mat prev_, curr_;
+	Mat camera_matrix_, dist_coeffs_;
+	tf2_ros::Buffer tf_buffer_;
+	tf2_ros::TransformListener tf_listener_;
+
+	void onInit()
+	{
+		ros::NodeHandle& nh = getNodeHandle();
+		ros::NodeHandle& nh_priv = getPrivateNodeHandle();
+		image_transport::ImageTransport it(nh);
+		image_transport::ImageTransport it_priv(nh_priv);
+
+		nh_priv.param<std::string>("mavros/local_position/tf/child_frame_id", fcu_frame_id_, "fcu");
+		nh_priv.param("roi", roi_, 128);
+		roi_2_ = roi_ / 2;
+
+		img_sub_ = it.subscribeCamera("image", 1, &OpticalFlow::flow, this);
+		img_pub_ = it_priv.advertise("debug", 1);
+		flow_pub_ = nh.advertise<mavros_msgs::OpticalFlowRad>("mavros/px4flow/raw/send", 1);
+		velo_pub_ = nh_priv.advertise<geometry_msgs::TwistStamped>("angular_velocity", 1);
+		shift_pub_ = nh_priv.advertise<geometry_msgs::Vector3Stamped>("shift", 1);
+
+		flow_.integrated_xgyro = NAN; // no IMU available
+		flow_.integrated_ygyro = NAN;
+		flow_.integrated_zgyro = NAN;
+		flow_.time_delta_distance_us = 0;
+		flow_.distance = -1; // no distance sensor available
+		flow_.temperature = 0;
+
+		ROS_INFO("Optical Flow initialized");
+	}
+
+	void parseCameraInfo(const sensor_msgs::CameraInfoConstPtr &cinfo) {
+		for (int i = 0; i < 3; ++i) {
+			for (int j = 0; j < 3; ++j) {
+				camera_matrix_.at<double>(i, j) = cinfo->K[3 * i + j];
+			}
+		}
+		for (int k = 0; k < cinfo->D.size(); k++) {
+			dist_coeffs_.at<double>(k) = cinfo->D[k];
+		}
+	}
+
+	void drawFlow(Mat& frame, double x, double y, double quality) const
+	{
+		double brightness = (1 - quality) * 25;;
+		cv::Scalar color(brightness, brightness, brightness);
+		double radius = std::sqrt(x * x + y * y);
+
+		// draw a circle and line indicating the shift direction...
+		cv::Point center(frame.cols >> 1, frame.rows >> 1);
+		cv::circle(frame, center, (int)(radius*5), color, 3, cv::LINE_AA);
+		cv::line(frame, center, cv::Point(center.x + (int)(x*5), center.y + (int)(y*5)), color, 3, cv::LINE_AA);
+	}
+
+	void flow(const sensor_msgs::ImageConstPtr& msg, const sensor_msgs::CameraInfoConstPtr& cinfo)
+	{
+		parseCameraInfo(cinfo);
+
+		auto img = cv_bridge::toCvShare(msg, "mono8")->image;
+
+		// Apply ROI
+		if (roi_ != 0) {
+			img = img(cv::Rect((msg->width / 2 - roi_2_), (msg->height / 2 - roi_2_), roi_, roi_));
+		}
+
+		img.convertTo(curr_, CV_64F);
+
+		if (prev_.empty()) {
+			prev_ = curr_.clone();
+			prev_stamp_ = msg->header.stamp;
+			cv::createHanningWindow(hann_, curr_.size(), CV_64F);
+
+		} else {
+			double response;
+			cv::Point2d shift = cv::phaseCorrelate(prev_, curr_, hann_, &response);
+
+			// Publish raw shift in pixels
+			static geometry_msgs::Vector3Stamped shift_vec;
+			shift_vec.header.stamp = msg->header.stamp;
+			shift_vec.header.frame_id = msg->header.frame_id;
+			shift_vec.vector.x = shift.x;
+			shift_vec.vector.y = shift.y;
+			shift_pub_.publish(shift_vec);
+
+			// Undistort flow in pixels
+			uint32_t flow_center_x = msg->width / 2;
+			uint32_t flow_center_y = msg->height / 2;
+			shift.x += flow_center_x;
+			shift.y += flow_center_y;
+
+			std::vector<cv::Point2d> points_dist = { shift };
+			std::vector<cv::Point2d> points_undist(1);
+
+			cv::undistortPoints(points_dist, points_undist, camera_matrix_, dist_coeffs_, cv::noArray(), camera_matrix_);
+			points_undist[0].x -= flow_center_x;
+			points_undist[0].y -= flow_center_y;
+
+			// Calculate flow in radians
+			double focal_length_x = camera_matrix_.at<double>(0, 0);
+			double focal_length_y = camera_matrix_.at<double>(1, 1);
+			double flow_x = atan2(points_undist[0].x, focal_length_x);
+			double flow_y = atan2(points_undist[0].y, focal_length_y);
+
+			// // Convert to FCU frame
+			static geometry_msgs::Vector3Stamped flow_camera, flow_fcu;
+			flow_camera.header.frame_id = msg->header.frame_id;
+			flow_camera.header.stamp = msg->header.stamp;
+			flow_camera.vector.x = flow_y; // +y means counter-clockwise rotation around Y axis
+			flow_camera.vector.y = -flow_x; // +x means clockwise rotation around X axis
+			tf_buffer_.transform(flow_camera, flow_fcu, fcu_frame_id_);
+
+			// Calculate integration time
+			ros::Duration integration_time = msg->header.stamp - prev_stamp_;
+			uint32_t integration_time_us = integration_time.toSec() * 1.0e6;
+
+			// Publish flow in fcu frame
+			flow_.header.stamp = /*prev_stamp_*/ msg->header.stamp;
+			flow_.integration_time_us = integration_time_us;
+			flow_.integrated_x = flow_fcu.vector.x;
+			flow_.integrated_y = flow_fcu.vector.y;
+			flow_.quality = (uint8_t)(response * 255);
+			flow_pub_.publish(flow_);
+
+			// Publish debug image
+			if (img_pub_.getNumSubscribers() > 0) {
+				// publish debug image
+				drawFlow(img, shift_vec.vector.x, shift_vec.vector.y, response);
+				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::MONO8;
+				out_msg.image = img;
+				img_pub_.publish(out_msg.toImageMsg());
+			}
+
+			// Publish estimated angular velocity
+			static geometry_msgs::TwistStamped velo;
+			velo.header.stamp = msg->header.stamp;
+			velo.header.frame_id = fcu_frame_id_;
+			velo.twist.angular.x = flow_.integrated_x / integration_time.toSec();
+			velo.twist.angular.y = flow_.integrated_y / integration_time.toSec();
+			velo_pub_.publish(velo);
+
+			prev_ = curr_.clone();
+			prev_stamp_ = msg->header.stamp;
+		}
+	}
+};
+
+PLUGINLIB_EXPORT_CLASS(OpticalFlow, nodelet::Nodelet)

clever/src/selfcheck.py

@@ -1,93 +1,192 @@
 #!/usr/bin/env python
 
+import math
+from subprocess import Popen, PIPE
+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 geometry_msgs.msg import PoseStamped
+from geometry_msgs.msg import PoseStamped, TwistStamped
 
 
 # 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)
 
 
 rospy.init_node('selfcheck')
 
 
+failures = []
+
+
+def failure(text, *args):
+    failures.append(text % args)
+
+
+def check(name):
+    def inner(fn):
+        def wrapper(*args, **kwargs):
+            failures[:] = []
+            try:
+                fn(*args, **kwargs)
+                for f in failures:
+                    rospy.logwarn('%s: %s', name, f)
+            except Exception as e:
+                traceback.print_exc()
+                rospy.logwarn('%s: exception occured', name)
+                return
+            if not failures:
+                rospy.loginfo('%s: OK', name)
+        return wrapper
+    return inner
+
+
+@check('FCU')
 def check_fcu():
     try:
         state = rospy.wait_for_message('mavros/state', State, timeout=3)
         if not state.connected:
-            raise Exception('No connection to the FCU')
-    except:
-        raise Exception('No mavros state')
+            failure('No connection to the FCU (check wiring)')
+    except rospy.ROSException:
+        failure('No MAVROS state (check wiring)')
 
 
+@check('Camera')
 def check_camera(name):
     try:
-        rospy.wait_for_message(name + '/image_raw', Image, timeout=3)
-    except:
-        raise Exception('No %s camera images' % name)
+        img = rospy.wait_for_message(name + '/image_raw', Image, timeout=1)
+    except rospy.ROSException:
+        failure('%s: No images (is the camera connected properly?)', name)
+        return
     try:
-        rospy.wait_for_message(name + '/camera_info', CameraInfo, timeout=3)
-    except:
-        raise Exception('No %s camera camera info' % name)
+        info = rospy.wait_for_message(name + '/camera_info', CameraInfo, timeout=1)
+    except rospy.ROSException:
+        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=3)
-    except:
-        raise Exception('No aruco_pose/debug topic')
+        rospy.wait_for_message('aruco_pose/debug', Image, timeout=1)
+    except rospy.ROSException:
+        failure('No aruco_pose/debug messages')
 
 
+@check('Simple offboard node')
 def check_simpleoffboard():
     try:
         rospy.wait_for_service('navigate', timeout=3)
         rospy.wait_for_service('get_telemetry', timeout=3)
         rospy.wait_for_service('land', timeout=3)
-    except:
-        raise Exception('No simple_offboard services')
+    except rospy.ROSException:
+        failure('No simple_offboard services')
 
 
+@check('IMU')
 def check_imu():
     try:
-        rospy.wait_for_message('mavros/imu/data', Imu, timeout=3)
-    except:
-        raise Exception('No IMU data')
+        rospy.wait_for_message('mavros/imu/data', Imu, timeout=1)
+    except rospy.ROSException:
+        failure('No IMU data (check flight controller calibration)')
 
 
+@check('Local position')
 def check_local_position():
     try:
-        rospy.wait_for_message('mavros/local_position/pose', PoseStamped, timeout=3)
-    except:
-        raise Exception('No local position')
+        rospy.wait_for_message('mavros/local_position/pose', PoseStamped, timeout=1)
+    except rospy.ROSException:
+        failure('No local position')
 
 
+@check('Velocity estimation')
+def check_velocity():
+    try:
+        velocity = rospy.wait_for_message('mavros/local_position/velocity', TwistStamped, timeout=1)
+        horiz = math.hypot(velocity.twist.linear.x, velocity.twist.linear.y)
+        vert = velocity.twist.linear.z
+        if abs(horiz) > 0.1:
+            failure('Horizontal velocity estimation is %.2f m/s; is copter staying still?' % horiz)
+        if abs(vert) > 0.1:
+            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')
+
+
+@check('Global position (GPS)')
 def check_global_position():
     try:
-        rospy.wait_for_message('mavros/global_position/global', PoseStamped, timeout=3)
-    except:
-        raise Exception('No global position')
+        rospy.wait_for_message('mavros/global_position/global', NavSatFix, timeout=2)
+    except rospy.ROSException:
+        failure('No global position')
 
 
-def check(name, fn):
-    try:
-        fn()
-        rospy.loginfo('%s: OK', name)
-    except Exception as e:
-        rospy.logwarn('%s: %s', name, str(e))
+@check('Boot duration')
+def check_boot_duration():
+    proc = Popen('systemd-analyze', stdout=PIPE)
+    proc.wait()
+    output = proc.communicate()[0]
+    r = re.compile(r'([\d\.]+)s$')
+    duration = float(r.search(output).groups()[0])
+    if duration > 15:
+        failure('long Raspbian boot duration: %ss (systemd-analyze for analyzing)', duration)
+
+
+@check('CPU usage')
+def check_cpu_usage():
+    WHITELIST = 'nodelet',
+    CMD = "top -n 1 -b -i | tail -n +8 | awk '{ printf(\"%-8s\\t%-8s\\t%-8s\\n\", $1, $9, $12); }'"
+    proc = Popen(CMD, stdout=PIPE, shell=True)
+    proc.wait()
+    output = proc.communicate()[0]
+    processes = output.split('\n')
+    for process in processes:
+        if not process:
+            continue
+        pid, cpu, cmd = process.split('\t')
+
+        if cmd.strip() not in WHITELIST and float(cpu) > 30:
+            failure('High CPU usage (%s%%) detected: %s (PID %s)',
+                    cpu.strip(), cmd.strip(), pid.strip())
 
 
 def selfcheck():
-    check('FCU', check_fcu)
-    check('Simple offboard node', check_simpleoffboard)
-    check('Main camera node', lambda: check_camera('main_camera'))
-    check('aruco_pose/debug topic', check_aruco)
-    check('IMU data', check_imu)
-    check('Local position', check_local_position)
-    check('Global position (GPS)', check_global_position)
+    check_fcu()
+    check_imu()
+    check_local_position()
+    check_velocity()
+    check_global_position()
+    check_camera('main_camera')
+    check_aruco()
+    check_simpleoffboard()
+    check_cpu_usage()
+    check_boot_duration()
 
 
 if __name__ == '__main__':

clever/src/simple_offboard.py

@@ -83,12 +83,13 @@ AUTO_ARM = AUTO_OFFBOARD and rospy.get_param('~auto_arm', True)
 OFFBOARD_TIMEOUT = rospy.Duration(rospy.get_param('~offboard_timeout', 3))
 ARM_TIMEOUT = rospy.Duration(rospy.get_param('~arm_timeout', 5))
 LOCAL_POSITION_TIMEOUT = rospy.Duration(rospy.get_param('~local_position_timeout', 0.5))
-NAVIGATE_AFTER_ARMED = rospy.Duration(rospy.get_param('~navigate_after_armed', False))
+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)
 
 
 def offboard_and_arm():
@@ -120,6 +121,8 @@ def offboard_and_arm():
 
 ps = PoseStamped()
 vs = Vector3Stamped()
+pt = PositionTarget()
+at = AttitudeTarget()
 
 
 BRAKE_TIME = rospy.Duration(0)
@@ -164,7 +167,7 @@ def get_publisher_and_message(req, stamp, continued=True, update_frame=True):
         if update_frame:
             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)
+            ps.pose.orientation = orientation_from_euler(0, 0, req.yaw, axes='sxyz')
             current_nav_finish = tf_buffer.transform(ps, LOCAL_FRAME, TRANSFORM_TIMEOUT)
 
             if isinstance(req, srv.NavigateGlobalRequest):
@@ -183,13 +186,14 @@ def get_publisher_and_message(req, stamp, continued=True, update_frame=True):
                                          current_nav_start_stamp, req.speed)
 
         yaw_rate_flag = math.isnan(req.yaw)
-        msg = PositionTarget(coordinate_frame=PT.FRAME_LOCAL_NED,
-                             type_mask=PT.IGNORE_VX + PT.IGNORE_VY + PT.IGNORE_VZ +
-                                       PT.IGNORE_AFX + PT.IGNORE_AFY + PT.IGNORE_AFZ +
-                                       (PT.IGNORE_YAW if yaw_rate_flag else PT.IGNORE_YAW_RATE),
-                             position=setpoint,
-                             yaw=euler_from_orientation(current_nav_finish.pose.orientation, 'szyx')[2] - math.pi / 2,
-                             yaw_rate=req.yaw_rate)
+        msg = pt
+        msg.coordinate_frame = PT.FRAME_LOCAL_NED
+        msg.type_mask = PT.IGNORE_VX + PT.IGNORE_VY + PT.IGNORE_VZ + \
+                        PT.IGNORE_AFX + PT.IGNORE_AFY + PT.IGNORE_AFZ + \
+                        (PT.IGNORE_YAW if yaw_rate_flag else PT.IGNORE_YAW_RATE)
+        msg.position = setpoint
+        msg.yaw = euler_from_orientation(current_nav_finish.pose.orientation, 'sxyz')[2]
+        msg.yaw_rate = req.yaw_rate
         return position_pub, msg
 
     elif isinstance(req, (srv.SetPositionRequest, srv.SetPositionGlobalRequest)):
@@ -202,13 +206,14 @@ def get_publisher_and_message(req, stamp, continued=True, update_frame=True):
             pose_local.pose.position.x, pose_local.pose.position.y = global_to_local(req.lat, req.lon)
 
         yaw_rate_flag = math.isnan(req.yaw)
-        msg = PositionTarget(coordinate_frame=PT.FRAME_LOCAL_NED,
-                             type_mask=PT.IGNORE_VX + PT.IGNORE_VY + PT.IGNORE_VZ +
-                                       PT.IGNORE_AFX + PT.IGNORE_AFY + PT.IGNORE_AFZ +
-                                       (PT.IGNORE_YAW if yaw_rate_flag else PT.IGNORE_YAW_RATE),
-                             position=pose_local.pose.position,
-                             yaw=euler_from_orientation(pose_local.pose.orientation, 'szyx')[2] - math.pi / 2,
-                             yaw_rate=req.yaw_rate)
+        msg = pt
+        msg.coordinate_frame = PT.FRAME_LOCAL_NED
+        msg.type_mask = PT.IGNORE_VX + PT.IGNORE_VY + PT.IGNORE_VZ + \
+                        PT.IGNORE_AFX + PT.IGNORE_AFY + PT.IGNORE_AFZ + \
+                        (PT.IGNORE_YAW if yaw_rate_flag else PT.IGNORE_YAW_RATE)
+        msg.position = pose_local.pose.position
+        msg.yaw = euler_from_orientation(pose_local.pose.orientation, 'sxyz')[2]
+        msg.yaw_rate = req.yaw_rate
         return position_pub, msg
 
     elif isinstance(req, srv.SetVelocityRequest):
@@ -220,28 +225,33 @@ def get_publisher_and_message(req, stamp, continued=True, update_frame=True):
         vector_local = tf_buffer.transform(vs, LOCAL_FRAME, TRANSFORM_TIMEOUT)
 
         yaw_rate_flag = math.isnan(req.yaw)
-        msg = PositionTarget(coordinate_frame=PT.FRAME_LOCAL_NED,
-                             type_mask=PT.IGNORE_PX + PT.IGNORE_PY + PT.IGNORE_PZ +
-                                       PT.IGNORE_AFX + PT.IGNORE_AFY + PT.IGNORE_AFZ +
-                                       (PT.IGNORE_YAW if yaw_rate_flag else PT.IGNORE_YAW_RATE),
-                             velocity=vector_local.vector,
-                             yaw=euler_from_orientation(pose_local.pose.orientation, 'szyx')[2] - math.pi / 2,
-                             yaw_rate=req.yaw_rate)
+        msg = pt
+        msg.coordinate_frame = PT.FRAME_LOCAL_NED
+        msg.type_mask = PT.IGNORE_PX + PT.IGNORE_PY + PT.IGNORE_PZ + \
+                                       PT.IGNORE_AFX + PT.IGNORE_AFY + PT.IGNORE_AFZ + \
+                                       (PT.IGNORE_YAW if yaw_rate_flag else PT.IGNORE_YAW_RATE)
+        msg.velocity = vector_local.vector
+        msg.yaw = euler_from_orientation(pose_local.pose.orientation, 'sxyz')[2]
+        msg.yaw_rate = req.yaw_rate
         return position_pub, msg
 
     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)
-        msg = AttitudeTarget(orientation=pose_local.pose.orientation,
-                             thrust=req.thrust,
-                             type_mask=AT.IGNORE_YAW_RATE + AT.IGNORE_PITCH_RATE + AT.IGNORE_ROLL_RATE)
+        msg = at
+        msg.orientation = pose_local.pose.orientation
+        msg.thrust = req.thrust
+        msg.type_mask = AT.IGNORE_YAW_RATE + AT.IGNORE_PITCH_RATE + AT.IGNORE_ROLL_RATE
         return attitude_pub, msg
 
     elif isinstance(req, srv.SetRatesRequest):
-        msg = AttitudeTarget(thrust=req.thrust,
-                             type_mask=AttitudeTarget.IGNORE_ATTITUDE,
-                             body_rate=Vector3(req.roll_rate, req.pitch_rate, req.yaw_rate))
+        msg = at
+        msg.thrust = req.thrust
+        msg.type_mask = AT.IGNORE_ATTITUDE
+        msg.body_rate.x = req.roll_rate
+        msg.body_rate.y = req.pitch_rate
+        msg.body_rate.z = req.yaw_rate
         return attitude_pub, msg
 
 
@@ -261,9 +271,12 @@ def handle(req):
         rospy.logwarn('No connection to the FCU')
         return {'message': 'No connection to the FCU'}
 
-    if isinstance(req, (srv.NavigateRequest, srv.NavigateGlobalRequest)) and req.speed <= 0:
-        rospy.logwarn('Navigate speed must be greater than zero, %s passed')
-        return {'message': 'Navigate speed must be greater than zero, %s passed' % req.speed}
+    if isinstance(req, (srv.NavigateRequest, srv.NavigateGlobalRequest)):
+        if req.speed < 0:
+            rospy.logwarn('Navigate speed must be positive, %s passed')
+            return {'message': 'Navigate speed must be positive, %s passed' % req.speed}
+        elif req.speed == 0:
+            req.speed = DEFAULT_SPEED
 
     if isinstance(req, (srv.NavigateRequest, srv.NavigateGlobalRequest)) and \
             (pose is None or rospy.get_rostime() - pose.header.stamp > LOCAL_POSITION_TIMEOUT):
@@ -280,13 +293,13 @@ def handle(req):
 
     try:
         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)
+            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)
 
-                current_msg.header.stamp = stamp
-                current_pub.publish(current_msg)
+            current_msg.header.stamp = stamp
+            current_pub.publish(current_msg)
 
         if req.auto_arm:
             offboard_and_arm()

clever/src/vl53l1x.py

@@ -0,0 +1,35 @@
+#!/usr/bin/env python
+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)
+
+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
+    range_pub.publish(msg)
+    r.sleep()
+
+tof.stop_ranging() # Stop ranging

clever/src/web_server.py

@@ -1,60 +0,0 @@
-#!/usr/bin/env python
-
-import rospy
-import subprocess
-import re
-from flask import Flask, send_from_directory, send_file, request, jsonify
-
-rospy.init_node('web_server', disable_signals=True)
-
-port = rospy.get_param('~port', 7070)
-host = rospy.get_param('~host', '0.0.0.0')
-serve_path = rospy.get_param('~path')
-app = Flask(__name__)
-
-
-@app.route('/')
-def serve_index():
-    return send_from_directory(serve_path, 'index.html')
-
-
-@app.route('/<path:path>')
-def serve_static(path):
-    print serve_path, path
-    return send_from_directory(serve_path, path)
-
-
-@app.route('/wifi_data/')
-def get_wifi_data():
-    cur_ip = request.remote_addr
-    ip_signal = get_ip_signal()
-    return jsonify({'ip': cur_ip, 'signal': ip_signal[cur_ip]}), 200
-
-
-def get_ip_signal():
-    wlan_interface = 'wlan0'
-    # Getting info about wifi client connected to access point. From here we know MAC and signal level
-    iwl = subprocess.check_output(['sudo', 'iw', 'dev', 'wlan0', 'station', 'dump']).splitlines()
-    mac_signal = {}
-    cur_client = ''
-    for line in iwl:
-        if line.find('Station') != -1:
-            cur_client = re.search(r'([0-9A-F]{2}[:-]){5}([0-9A-F]{2})', line, re.I).group()
-        if line.find('signal') != -1:
-            sg = re.search(r'(\[-?\d*\])', line, re.I).group()
-            mac_signal[cur_client] = re.sub(r'[\[\]]', '', sg)
-    ip_signal = {}
-    # Getting ip-mac mapping
-    ip_mac = subprocess.check_output(['arp', '-i', wlan_interface]).splitlines()
-    for line in ip_mac:
-        mac = re.search(r'([0-9A-F]{2}[:-]){5}([0-9A-F]{2})', line, re.I)
-        if mac is not None:
-            mac = mac.group()
-            if mac in mac_signal:
-                ips = re.search(r'((2[0-5]|1[0-9]|[0-9])?[0-9]\.){3}((2[0-5]|1[0-9]|[0-9])?[0-9])', line, re.I).group()
-                ip_signal[ips] = mac_signal[mac]
-    return ip_signal
-
-
-rospy.loginfo('Serving on %s:%s', host, port)
-app.run(host=host, port=port, threaded=True)