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
2026-06-10 03:24:32 +00:00 · 2018-10-01 20:06:01 +03:00
parent affd349ee9 aca11bdb40
commit efd9d82131
114 changed files with 1391 additions and 458 deletions
--- a/clever/CMakeLists.txt
+++ b/clever/CMakeLists.txt
@@ -21,6 +21,9 @@ find_package(catkin REQUIRED COMPONENTS
  tf
  tf2
  tf2_geometry_msgs
+  tf2_ros
+  image_transport
+  cv_bridge
 )


@@ -121,7 +124,7 @@ generate_messages(
 ## DEPENDS: system dependencies of this project that dependent projects also need
 catkin_package(
 #  INCLUDE_DIRS include
-#  LIBRARIES clever
+  LIBRARIES clever
 #  CATKIN_DEPENDS other_catkin_pkg
 #  DEPENDS system_lib
 )
@@ -137,7 +140,11 @@ include_directories(
  ${catkin_INCLUDE_DIRS}
 )

-## Declare a C++ library
+# Declare a C++ library
+add_library(clever
+  src/optical_flow.cpp
+)
+
 add_library(fcu_horiz
  src/fcu_horiz.cpp
 )
@@ -156,8 +163,16 @@ add_library(aruco_vpe
 ## The recommended prefix ensures that target names across packages don't collide
 add_executable(rc src/rc.cpp)

+add_executable(camera_markers src/camera_markers.cpp)
+
+add_executable(frames src/frames.cpp)
+
 target_link_libraries(rc ${catkin_LIBRARIES})

+target_link_libraries(camera_markers ${catkin_LIBRARIES})
+
+target_link_libraries(frames ${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
@@ -169,6 +184,10 @@ target_link_libraries(rc ${catkin_LIBRARIES})
 # add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})

 ## Specify libraries to link a library or executable target against
+target_link_libraries(clever
+  ${catkin_LIBRARIES}
+)
+
 target_link_libraries(fcu_horiz
  ${catkin_LIBRARIES}
  "/opt/ros/kinetic/lib/libtf2_ros.so"
--- a/clever/launch/clever.launch
+++ b/clever/launch/clever.launch
@@ -2,27 +2,27 @@
    <arg name="fcu_conn" default="usb"/>
    <arg name="fcu_ip" default="127.0.0.1"/>
    <arg name="gcs_bridge" default="tcp"/>
-    <arg name="viz" default="true"/>
-    <arg name="web_server" default="false"/>
+    <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="fpv_camera" default="false"/>
-    <arg name="fpv_camera_device" default="/dev/v4l/by-id/usb-HD_Camera_Manufacturer_USB_2.0_Camera-video-index0"/>
    <arg name="arduino" default="false"/>
+    <arg name="vl53l1x" default="false"/>

    <!-- mavros -->
    <include file="$(find clever)/launch/mavros.launch">
        <arg name="fcu_conn" value="$(arg fcu_conn)"/>
        <arg name="fcu_ip" value="$(arg fcu_ip)"/>
        <arg name="gcs_bridge" value="$(arg gcs_bridge)"/>
-        <arg name="viz" value="$(arg viz)"/>
+        <arg name="viz" value="true"/>
    </include>

-    <!-- web server -->
-    <include file="$(find clever)/launch/web_server.launch" if="$(arg web_server)"/>
+
+    <!-- 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"/>
@@ -30,6 +30,12 @@
    <!-- aruco vpe -->
    <include file="$(find clever)/launch/aruco.launch" if="$(arg aruco)"/>

+    <!-- optical flow -->
+    <node pkg="nodelet" type="nodelet" name="optical_flow" args="load clever/optical_flow nodelet_manager" if="$(arg optical_flow)" clear_params="true">
+        <remap from="image" to="main_camera/image_raw"/>
+        <remap from="camera_info" to="main_camera/camera_info"/>
+    </node>
+
    <!-- 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"/>
@@ -37,11 +43,12 @@

    <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"/>
+

    <!-- main camera -->
    <include file="$(find clever)/launch/main_camera.launch" if="$(arg main_camera)"/>
@@ -49,14 +56,14 @@
    <!-- 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)">
+        <!-- <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)"/>

-    <!-- FPV video streaming -->
-    <include file="$(find clever)/launch/fpv_camera.launch" if="$(arg fpv_camera)">
-        <arg name="device" value="$(arg fpv_camera_device)"/>
-    </include>
-
    <!-- Arduino bridge -->
    <include file="$(find clever)/launch/arduino.launch" if="$(arg arduino)"/>
 </launch>
--- a/clever/launch/copter_visualization.launch
+++ b/clever/launch/copter_visualization.launch
@@ -1,12 +0,0 @@
-<launch>
-    <remap to="mavros/local_position/pose" from="local_position"/>
-    <remap to="mavros/setpoint_position/local" from="local_setpoint"/>
-
-    <node name="copter_visualization" pkg="mavros_extras" type="copter_visualization"/>
-
-    <param name="copter_visualization/fixed_frame_id" value="local_origin"/>
-    <param name="copter_visualization/child_frame_id" value="fcu"/>
-    <param name="copter_visualization/marker_scale" value="1"/>
-    <param name="copter_visualization/max_track_size" value="500"/>
-    <param name="copter_visualization/num_rotors" value="4"/>
-</launch>
--- a/clever/launch/main_camera.launch
+++ b/clever/launch/main_camera.launch
@@ -1,4 +1,7 @@
 <launch>
+    <!-- Camera position and orientation are represented by fcu -> main_camera_optical transform -->
+    <!-- static_transform_publisher arguments: x y z yaw pitch roll frame_id child_frame_id -->
+
    <!-- clever 2 -->
    <!--<node pkg="tf2_ros" type="static_transform_publisher" name="main_camera_frame" args="0 0 -0.07 -1.5707963 0 3.1415926 fcu main_camera_optical"/>-->

@@ -16,8 +19,14 @@
        <!-- 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"/>
    </node>
+
+    <!-- camera visualization markers -->
+    <node pkg="clever" type="camera_markers" ns="main_camera" name="main_camera_markers">
+        <param name="scale" value="3.0"/>
+    </node>
 </launch>
--- a/clever/launch/mavros.launch
+++ b/clever/launch/mavros.launch
@@ -1,58 +1,96 @@
 <launch>
-    <arg name="fcu_conn" default="uart"/>
+    <arg name="fcu_conn" default="usb"/>
    <arg name="fcu_ip" default="127.0.0.1"/>
    <arg name="gcs_bridge" default="tcp"/>
    <arg name="viz" default="true"/>
    <arg name="respawn" default="true"/>

    <node pkg="mavros" type="mavros_node" name="mavros" required="false" clear_params="true" respawn="$(arg respawn)" respawn_delay="5" output="screen">
-            <!-- UART connection -->
-            <param name="fcu_url" value="/dev/ttyAMA0:921600" if="$(eval fcu_conn is None or fcu_conn == 'uart')"/>
+        <!-- UART connection -->
+        <param name="fcu_url" value="/dev/ttyAMA0:921600" if="$(eval fcu_conn is None or fcu_conn == 'uart')"/>

-            <!-- USB connection -->
-            <param name="fcu_url" value="/dev/ttyACM0" if="$(eval fcu_conn == 'usb')"/>
+        <!-- USB connection -->
+        <param name="fcu_url" value="/dev/ttyACM0" if="$(eval fcu_conn == 'usb')"/>

-            <!-- sitl -->
-            <param name="fcu_url" value="udp://@$(arg fcu_ip):14557" if="$(eval fcu_conn == 'udp')"/>
+        <!-- sitl -->
+        <param name="fcu_url" value="udp://@$(arg fcu_ip):14557" if="$(eval fcu_conn == 'udp')"/>

-            <!-- gcs bridge -->
-            <param name="gcs_url" value="tcp-l://0.0.0.0:5760" if="$(eval gcs_bridge == 'tcp')"/>
-            <param name="gcs_url" value="udp://0.0.0.0:14550@14550" if="$(eval gcs_bridge == 'udp')"/>
-            <param name="gcs_url" value="udp-b://$(env ROS_IP):14550@14550" if="$(eval gcs_bridge == 'udp-b')"/>
-            <param name="gcs_url" value="udp-pb://$(env ROS_IP):14550@14550" if="$(eval gcs_bridge == 'udp-pb')"/>
-            <param name="gcs_url" value="" if="$(eval not gcs_bridge)"/>
-            <param name="gcs_quiet_mode" value="true"/>
-            <param name="conn/timeout" value="8"/>
+        <!-- gcs bridge -->
+        <param name="gcs_url" value="tcp-l://0.0.0.0:5760" if="$(eval gcs_bridge == 'tcp')"/>
+        <param name="gcs_url" value="udp://0.0.0.0:14550@14550" if="$(eval gcs_bridge == 'udp')"/>
+        <param name="gcs_url" value="udp-b://$(env ROS_IP):14550@14550" if="$(eval gcs_bridge == 'udp-b')"/>
+        <param name="gcs_url" value="udp-pb://$(env ROS_IP):14550@14550" if="$(eval gcs_bridge == 'udp-pb')"/>
+        <param name="gcs_url" value="" if="$(eval not gcs_bridge)"/>
+        <param name="gcs_quiet_mode" value="true"/>
+        <param name="conn/timeout" value="8"/>

-            <!-- default px4 params -->
-            <rosparam command="load" file="$(find mavros)/launch/px4_config.yaml"/>
+        <!-- default px4 params -->
+        <rosparam command="load" file="$(find mavros)/launch/px4_config.yaml"/>

-            <!-- additional params -->
-            <param name="local_position/frame_id" value="local_origin"/>
-            <param name="local_position/tf/send" value="true"/>
-            <param name="local_position/tf/frame_id" value="local_origin"/>
-            <param name="local_position/tf/child_frame_id" value="fcu"/>
-            <param name="global_position/tf/send" value="false"/>
-            <param name="imu/frame_id" value="fcu"/>
-            <rosparam param="plugin_blacklist">
-                - safety_area
-                - image_pub
-                - vibration
-                - distance_sensor
-                - rangefinder
-                - 3dr_radio
-                - actuator_control
-                - hil_controls
-                - vfr_hud
-                - px4flow
-                - vision_speed_estimate
-                - fake_gps
-                - cam_imu_sync
-                - hil
-                - adsb
-            </rosparam>
+        <!-- rangefinders -->
+        <rosparam>
+            distance_sensor:
+                rangefinder_0:
+                    id: 0
+                    frame_id: "rangefinder"
+                    orientation: PITCH_270
+                    field_of_view: 0.5
+                rangefinder_1:
+                    id: 1
+                    frame_id: "rangefinder"
+                    orientation: PITCH_270
+                    field_of_view: 0.5
+                rangefinder_2_sub:
+                    subscriber: true
+                    id: 2
+                    orientation: PITCH_270
+                rangefinder_3_sub:
+                    subscriber: true
+                    id: 3
+                    orientation: PITCH_270
+        </rosparam>
+
+        <!-- additional params -->
+        <param name="local_position/frame_id" value="local_origin"/>
+        <param name="local_position/tf/send" value="true"/>
+        <param name="local_position/tf/frame_id" value="local_origin"/>
+        <param name="local_position/tf/child_frame_id" value="fcu"/>
+        <param name="global_position/tf/send" value="false"/>
+        <param name="imu/frame_id" value="fcu"/>
+        <rosparam param="plugin_blacklist">
+            - safety_area
+            - image_pub
+            - vibration
+            - rangefinder
+            - 3dr_radio
+            - actuator_control
+            - hil_controls
+            - vfr_hud
+            - vision_speed_estimate
+            - fake_gps
+            - cam_imu_sync
+            - hil
+            - adsb
+            - waypoint
+            - obstacle_distance
+            - setpoint_accel
+            - trajectory
+            - wind_estimation
+            - home_position
+        </rosparam>
    </node>

+    <!-- Rangefinders frame -->
+    <node pkg="tf2_ros" type="static_transform_publisher" name="rangefinder_frame" args="0 0 -0.05 0 1.5707963268 0 fcu rangefinder"/>
+
    <!-- Copter visualization -->
-    <include file="$(find clever)/launch/copter_visualization.launch" if="$(arg viz)"/>
+    <node name="copter_visualization" pkg="mavros_extras" type="copter_visualization" if="$(arg viz)">
+        <remap to="mavros/local_position/pose" from="local_position"/>
+        <remap to="mavros/setpoint_position/local" from="local_setpoint"/>
+        <param name="fixed_frame_id" value="local_origin"/>
+        <param name="child_frame_id" value="fcu"/>
+        <param name="marker_scale" value="1"/>
+        <param name="max_track_size" value="20"/>
+        <param name="num_rotors" value="4"/>
+    </node>
 </launch>
--- a/clever/launch/sitl.launch
+++ b/clever/launch/sitl.launch
@@ -7,11 +7,11 @@
        <arg name="fcu_conn" value="udp"/>
        <arg name="fcu_ip" value="$(arg ip)"/>
        <arg name="gcs_bridge" value="false"/>
+        <arg name="optical_flow" value="false"/>
+        <arg name="web_server" default="false"/>
        <arg name="web_video_server" default="false"/>
        <arg name="main_camera" default="false"/>
-        <arg name="fpv_camera" default="false"/>
        <arg name="rosbridge" value="$(arg rosbridge)"/>
-        <arg name="web_server" default="false"/>
        <arg name="aruco" default="false"/>
    </include>
 </launch>
--- a/clever/launch/web_server.launch
+++ b/clever/launch/web_server.launch
@@ -1,5 +0,0 @@
-<launch>
-    <node name="web_server" pkg="clever" type="web_server.py" output="screen">
-        <param name="path" value="$(find clever)/static"/>
-    </node>
-</launch>
--- a/clever/nodelet_plugins.xml
+++ b/clever/nodelet_plugins.xml
@@ -1,3 +1,8 @@
+<library path="lib/libclever">
+   <class name="clever/optical_flow" type="OpticalFlow" base_class_type="nodelet::Nodelet">
+      <description/>
+   </class>
+</library>
 <library path="lib/libfcu_horiz">
   <class name="clever/fcu_horiz" type="FcuHoriz" base_class_type="nodelet::Nodelet">
      <description/>
--- a/clever/requirements.txt
+++ b/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
--- a/clever/src/camera_markers.cpp
+++ b/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();
+}
--- a/clever/src/frames.cpp
+++ b/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();
+}
--- a/clever/src/interactive.py
+++ b/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()
--- a/clever/src/monkey
+++ b/clever/src/monkey
@@ -0,0 +1,3 @@
+#!/usr/bin/env bash
+
+exec /home/pi/monkey/build/monkey --port 80 --workers 1
--- a/clever/src/optical_flow.cpp
+++ b/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)
--- a/clever/src/selfcheck.py
+++ b/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__':
--- a/clever/src/simple_offboard.py
+++ b/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()
--- a/clever/src/vl53l1x.py
+++ b/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
--- a/clever/src/web_server.py
+++ b/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)
--- a/clever/static/index.html
+++ b/clever/static/index.html
@@ -0,0 +1,13 @@
+<h1>CLEVER Drone Kit Tools</h1>
+
+<ul>
+	<!-- <li><a href="">View user reference</a> (<a href="http://clever.copterexpress.com">http://clever.copterexpress.com</a> snapshot)</li> -->
+	<li><a href="" id="wvs">View image topics</a> (<code>web_video_server</code>)</li>
+	<li><a href="" id="butterfly">Open web terminal</a> (<code>Butterfly</code>)</li>
+	<!-- <li><a href="viz.html">View 3D visualization</a> (<code>ros3djs</code>)</li> -->
+</ul>
+
+<script type="text/javascript">
+	document.querySelector("#wvs").href = location.origin + ':8080';
+	document.querySelector("#butterfly").href = location.origin + ':57575';
+</script>