simple_offboard: implement navigate velocity feedforward

aruco_pose/src/aruco_detect.cpp

@@ -71,8 +71,7 @@ private:
 	ros::Publisher markers_pub_, vis_markers_pub_;
 	ros::Subscriber map_markers_sub_;
 	ros::ServiceServer set_markers_srv_;
-	bool estimate_poses_, send_tf_, auto_flip_, use_map_markers_;
+	bool estimate_poses_, send_tf_, auto_flip_;
-	bool waiting_for_map_;
 	double length_;
 	ros::Duration transform_timeout_;
 	std::unordered_map<int, double> length_override_;
@@ -96,8 +95,6 @@ public:
 		dictionary = nh_priv_.param("dictionary", 2);
 		estimate_poses_ = nh_priv_.param("estimate_poses", true);
 		send_tf_ = nh_priv_.param("send_tf", true);
-		use_map_markers_ = nh_priv_.param("use_map_markers", false);
-		waiting_for_map_ = use_map_markers_;
 		if (estimate_poses_ && !nh_priv_.getParam("length", length_)) {
 			NODELET_FATAL("can't estimate marker's poses as ~length parameter is not defined");
 			ros::shutdown();
@@ -136,7 +133,6 @@ private:
 	void imageCallback(const sensor_msgs::ImageConstPtr& msg, const sensor_msgs::CameraInfoConstPtr &cinfo)
 	{
 		if (!enabled_) return;
-		if (waiting_for_map_) return;
 
 		Mat image = cv_bridge::toCvShare(msg, "bgr8")->image;
 
@@ -399,13 +395,7 @@ private:
 		map_markers_ids_.clear();
 		for (auto const& marker : msg.markers) {
 			map_markers_ids_.insert(marker.id);
-			if (use_map_markers_) {
-				if (length_override_.find(marker.id) == length_override_.end()) {
-					length_override_[marker.id] = marker.length;
-				}
-			}
 		}
-		waiting_for_map_ = false;
 	}
 
 	void paramCallback(aruco_pose::DetectorConfig &config, uint32_t level)

aruco_pose/src/aruco_map.cpp

@@ -503,7 +503,7 @@ publish_debug:
 		vis_marker.pose.position.x = x;
 		vis_marker.pose.position.y = y;
 		vis_marker.pose.position.z = z;
-		tf::quaternionTFToMsg(q, vis_marker.pose.orientation);
+		tf::quaternionTFToMsg(q, marker.pose.orientation);
 		vis_marker.frame_locked = true;
 		vis_array_.markers.push_back(vis_marker);
 

aruco_pose/test/basic.py

@@ -6,7 +6,7 @@ import tf2_geometry_msgs
 from geometry_msgs.msg import PoseWithCovarianceStamped
 from sensor_msgs.msg import Image
 from aruco_pose.msg import MarkerArray
-from visualization_msgs.msg import MarkerArray as VisMarkerArray, Marker as VisMarker
+from visualization_msgs.msg import MarkerArray as VisMarkerArray
 
 
 @pytest.fixture
@@ -199,36 +199,6 @@ def test_map_markers(node):
 
 def test_map_visualization(node):
     vis = rospy.wait_for_message('aruco_map/visualization', VisMarkerArray, timeout=5)
-    assert len(vis.markers) == 7
-    assert vis.markers[0].header.frame_id == 'aruco_map'
-    assert vis.markers[0].type == VisMarker.CUBE
-    assert vis.markers[0].action == VisMarker.ADD
-    assert vis.markers[0].pose.position.x == 0
-    assert vis.markers[0].pose.position.y == 0
-    assert vis.markers[0].pose.position.z == 0
-    assert vis.markers[0].pose.orientation.x == 0
-    assert vis.markers[0].pose.orientation.y == 0
-    assert vis.markers[0].pose.orientation.z == 0
-    assert vis.markers[0].pose.orientation.w == 1
-    assert vis.markers[0].scale.x == approx(0.33)
-    assert vis.markers[0].scale.y == approx(0.33)
-    assert vis.markers[0].scale.z == approx(0.001)
-    assert vis.markers[1].pose.position.x == 1
-    assert vis.markers[1].pose.position.y == 0
-    assert vis.markers[1].pose.position.z == 0
-    assert vis.markers[1].pose.orientation.x == 0
-    assert vis.markers[1].pose.orientation.y == 0
-    assert vis.markers[1].pose.orientation.z == 0
-    assert vis.markers[1].pose.orientation.w == 1
-    # non-zero yaw marker:
-    assert vis.markers[4].scale.x == approx(0.5)
-    assert vis.markers[4].pose.position.x == approx(0.5)
-    assert vis.markers[4].pose.position.y == 2
-    assert vis.markers[4].pose.position.z == 0
-    assert vis.markers[4].pose.orientation.x == 0
-    assert vis.markers[4].pose.orientation.y == 0
-    assert vis.markers[4].pose.orientation.z == approx(0.5646424733950354)
-    assert vis.markers[4].pose.orientation.w == approx(0.8253356149096783)
 
 def test_map_debug(node):
     img = rospy.wait_for_message('aruco_map/debug', Image, timeout=5)

builder/image-ros.sh

@@ -112,7 +112,7 @@ my_travis_retry pip3 install wheel
 my_travis_retry pip3 install -r /home/pi/catkin_ws/src/clover/clover/requirements.txt
 source /opt/ros/${ROS_DISTRO}/setup.bash
 # Don't build simulation plugins for actual drone
-sudo -E -u pi sh -c '. /opt/ros/${ROS_DISTRO}/setup.sh && catkin_make -j2 -DCMAKE_BUILD_TYPE=RelWithDebInfo'
+catkin_make -j2 -DCMAKE_BUILD_TYPE=RelWithDebInfo
 source devel/setup.bash
 
 echo_stamp "Install clever package (for backwards compatibility)"

builder/test/tests.py

@@ -2,7 +2,6 @@
 
 # validate all required modules installed
 
-import os
 import rospy
 from geometry_msgs.msg import PoseStamped
 from sensor_msgs.msg import Range, BatteryState
@@ -23,7 +22,6 @@ from clover.srv import GetTelemetry, Navigate, NavigateGlobal, SetPosition, SetV
 from led_msgs.srv import SetLEDs
 from led_msgs.msg import LEDStateArray, LEDState
 from aruco_pose.msg import Marker, MarkerArray, Point2D
-from clover import long_callback
 
 import dynamic_reconfigure.client
 
@@ -33,11 +31,9 @@ import tf2_geometry_msgs
 import VL53L1X
 import pymavlink
 from pymavlink import mavutil
+import rpi_ws281x
+import pigpio
 # from espeak import espeak
 from pyzbar import pyzbar
 
 print(cv2.getBuildInformation())
-
-if not os.environ.get('VM'):
-    import rpi_ws281x
-    import pigpio

builder/test/tests.sh

@@ -6,10 +6,16 @@ set -ex
 
 # validate required software is installed
 
+python --version
 python2 --version
 python3 --version
+ipython --version
 ipython3 --version
 
+# ptvsd does not have a stand-alone binary
+python -m ptvsd --version
+python3 -m ptvsd --version
+
 node -v
 npm -v
 
@@ -19,64 +25,42 @@ lsof -v
 git --version
 vim --version
 pip --version
+pip2 --version
 pip3 --version
 tcpdump --version
 monkey --version
+pigpiod -v
+i2cdetect -V
+butterfly -h
 # espeak --version
+mjpg_streamer --version
 systemctl --version
 
-if [ -z $VM ]; then
-	# rpi only software
-	python --version
-	ipython --version
-	pip2 --version
-	# `python` is python2 for now
-	[[ $(python -c 'import sys;print(sys.version_info.major)') == "2" ]]
-
-	# ptvsd does not have a stand-alone binary
-	python -m ptvsd --version
-	python3 -m ptvsd --version
-
-	pigpiod -v
-	i2cdetect -V
-	butterfly -h
-	mjpg_streamer --version
-fi
-
 # ros stuff
 
 roscore -h
 rosversion clover
 rosversion aruco_pose
+rosversion vl53l1x
 rosversion mavros
 rosversion mavros_extras
 rosversion ws281x
 rosversion led_msgs
 rosversion dynamic_reconfigure
 rosversion tf2_web_republisher
-rosversion rosbridge_server
+rosversion compressed_image_transport
+rosversion rosbridge_suite
+rosversion rosserial
 rosversion usb_cam
 rosversion cv_camera
 rosversion web_video_server
+rosversion rosshow
 rosversion nodelet
 rosversion image_view
 
+# validate some versions
+[[ $(rosversion cv_camera) == "0.5.1" ]] # patched version with init fix
 [[ $(rosversion ws281x) == "0.0.13" ]]
 
-if [ -z $VM ]; then
-	rosversion compressed_image_transport
-	rosversion rosshow
-	rosversion vl53l1x
-	rosversion rosserial
-	[[ $(rosversion cv_camera) == "0.5.1" ]] # patched version with init fix
-fi
-
-[ $VM ] && H="/home/clover" || H="/home/pi"
-
 # validate examples are present
-[[ $(ls $H/examples/*) ]]
+[[ $(ls /home/pi/examples/*) ]]
-
-# validate web tools present
-[ -d $H/.ros/www ]
-[ "$(readlink $H/.ros/www/clover)" = "$H/catkin_ws/src/clover/clover/www" ]
-[ "$(readlink $H/.ros/www/clover_blocks)" = "$H/catkin_ws/src/clover/clover_blocks/www" ]

clover/CMakeLists.txt

@@ -53,7 +53,7 @@ find_package(OpenCV ${_opencv_version} REQUIRED
 ## Uncomment this if the package has a setup.py. This macro ensures
 ## modules and global scripts declared therein get installed
 ## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html
-catkin_python_setup()
+# catkin_python_setup()
 
 ################################################
 ## Declare ROS messages, services and actions ##
@@ -230,9 +230,6 @@ target_link_libraries(${PROJECT_NAME}
   ${OpenCV_LIBRARIES}
 )
 
-# Set Clover to default www page
-set(ROSWWW_STATIC_DEFAULT ${PROJECT_NAME})
-
 #############
 ## Install ##
 #############

clover/examples/camera.py

@@ -1,64 +0,0 @@
-# Information: https://clover.coex.tech/camera
-
-# Example on basic working with the camera and image processing:
-
-# - cuts out a central square from the camera image;
-# - publishes this cropped image to the topic `/cv/center`;
-# - computes the average color of it;
-# - prints its name to the console. 
-
-import rospy
-import cv2
-from sensor_msgs.msg import Image
-from cv_bridge import CvBridge
-from clover import long_callback
-
-rospy.init_node('cv')
-bridge = CvBridge()
-
-printed_color = None
-center_pub = rospy.Publisher('~center', Image, queue_size=1)
-
-def get_color_name(h):
-    if h < 15: return 'red'
-    if h < 30: return 'orange'
-    elif h < 60: return 'yellow'
-    elif h < 90: return 'green'
-    elif h < 120: return 'cyan'
-    elif h < 150: return 'blue'
-    elif h < 170: return 'magenta'
-    else: return 'red'
-
-
-@long_callback
-def image_callback(msg):
-    img = bridge.imgmsg_to_cv2(msg, 'bgr8')
-
-    # convert to HSV to work with color hue
-    img_hsv = cv2.cvtColor(img, cv2.COLOR_BGR2HSV)
-
-    # cut out a central square
-    w = img.shape[1]
-    h = img.shape[0]
-    r = 20
-    center = img_hsv[h // 2 - r:h // 2 + r, w // 2 - r:w // 2 + r]
-
-    # compute and print the average hue
-    mean_hue = center[:, :, 0].mean()
-    color = get_color_name(mean_hue)
-    global printed_color
-    if color != printed_color:
-        print(color)
-        printed_color = color
-
-    # publish the cropped image
-    center = cv2.cvtColor(center, cv2.COLOR_HSV2BGR)
-    center_pub.publish(bridge.cv2_to_imgmsg(center, 'bgr8'))
-
-# process every frame:
-image_sub = rospy.Subscriber('main_camera/image_raw', Image, image_callback, queue_size=1)
-
-# process 5 frames per second:
-# image_sub = rospy.Subscriber('main_camera/image_raw_throttled', Image, image_callback, queue_size=1)
-
-rospy.spin()

clover/launch/aruco.launch

@@ -18,7 +18,6 @@
         <remap from="map_markers" to="aruco_map/map"/>
         <param name="estimate_poses" value="true"/>
         <param name="send_tf" value="true"/>
-        <param name="use_map_markers" value="true"/>
         <param name="known_tilt" value="map" if="$(eval placement == 'floor')"/>
         <param name="known_tilt" value="map_flipped" if="$(eval placement == 'ceiling')"/>
         <param name="length" value="$(arg length)"/>
@@ -53,8 +52,4 @@
         <param name="force_init" value="$(arg force_init)"/>
         <param name="offset_frame_id" value="aruco_map"/>
     </node>
-
-    <!-- run map_flipped frame if placement is ceiling -->
-    <node pkg="tf2_ros" type="static_transform_publisher" name="map_flipped_frame"
-        args="0 0 0 3.1415926 3.1415926 0 map map_flipped" if="$(eval placement == 'ceiling')"/>
 </launch>

clover/launch/clover.launch

@@ -45,9 +45,10 @@
         <remap from="camera_info" to="main_camera/camera_info"/>
         <param name="calc_flow_gyro" value="true"/>
         <param name="roi_rad" value="0.8"/>
-        <param name="disable_on_vpe" value="false"/>
     </node>
 
+    <node pkg="tf2_ros" type="static_transform_publisher" name="map_flipped_frame" args="0 0 0 3.1415926 3.1415926 0 map map_flipped"/>
+
     <!-- simplified offboard control -->
     <node name="simple_offboard" pkg="clover" type="simple_offboard" output="screen" clear_params="true">
         <param name="reference_frames/main_camera_optical" value="map"/>
@@ -84,4 +85,9 @@
         <!-- Send fake GCS heartbeats. Set to "true" for upstream PX4 -->
         <param name="use_fake_gcs" value="false"/>
     </node>
+
+    <!-- Update static directory -->
+    <node pkg="roswww_static" name="roswww_static" type="main.py" clear_params="true">
+        <param name="default_package" value="clover"/>
+    </node>
 </launch>

clover/setup.py

@@ -1,11 +0,0 @@
-## ! DO NOT MANUALLY INVOKE THIS setup.py, USE CATKIN INSTEAD
-
-from distutils.core import setup
-from catkin_pkg.python_setup import generate_distutils_setup
-
-# fetch values from package.xml
-setup_args = generate_distutils_setup(
-    packages=['clover'],
-    package_dir={'': 'src'})
-
-setup(**setup_args)

clover/src/clover/__init__.py

@@ -1,35 +0,0 @@
-import rospy
-from threading import Thread, Event
-
-def long_callback(fn):
-    """
-    Decorator fixing a rospy issue for long-running topic callbacks, primarily
-    for image processing.
-
-    See: https://github.com/ros/ros_comm/issues/1901.
-
-    Usage example:
-
-    @long_callback
-    def image_callback(msg):
-        # perform image processing
-        # ...
-
-    rospy.Subscriber('main_camera/image_raw', Image, image_callback, queue_size=1)
-    """
-    e = Event()
-
-    def thread():
-        while not rospy.is_shutdown():
-            e.wait()
-            e.clear()
-            fn(thread.current_msg)
-
-    thread.current_msg = None
-    Thread(target=thread, daemon=True).start()
-
-    def wrapper(msg):
-        thread.current_msg = msg
-        e.set()
-
-    return wrapper

clover/src/led.cpp

@@ -319,8 +319,8 @@ int main(int argc, char **argv)
 
 	auto set_effect = nh.advertiseService("set_effect", &setEffect);
 
-	auto mavros_state_sub = nh.subscribe("mavros/state", 1, &handleMavrosState);
+	auto mavros_state_sub = nh.subscribe("/mavros/state", 1, &handleMavrosState);
-	auto battery_sub = nh.subscribe("mavros/battery", 1, &handleBattery);
+	auto battery_sub = nh.subscribe("/mavros/battery", 1, &handleBattery);
 	auto rosout_sub = nh.subscribe("/rosout_agg", 1, &handleLog);
 
 	timer = nh.createTimer(ros::Duration(0), &proceed, false, false);

clover/src/optical_flow.cpp

@@ -25,7 +25,6 @@
 #include <dynamic_reconfigure/server.h>
 #include <mavros_msgs/OpticalFlowRad.h>
 #include <sensor_msgs/Imu.h>
-#include <geometry_msgs/PoseStamped.h>
 #include <geometry_msgs/Vector3Stamped.h>
 #include <geometry_msgs/PointStamped.h>
 #include <geometry_msgs/TwistStamped.h>
@@ -58,9 +57,6 @@ private:
 	std::unique_ptr<tf2_ros::TransformListener> tf_listener_;
 	bool calc_flow_gyro_;
 	float flow_gyro_default_;
-	bool disable_on_vpe_;
-	ros::Subscriber vpe_sub_;
-	ros::Time last_vpe_time_;
 	std::shared_ptr<dynamic_reconfigure::Server<clover::FlowConfig>> dyn_srv_;
 
 	void onInit()
@@ -91,11 +87,6 @@ private:
 
 		img_sub_ = it.subscribeCamera("image_raw", 1, &OpticalFlow::flow, this);
 
-		disable_on_vpe_ = nh_priv.param("disable_on_vpe", false);
-		if (disable_on_vpe_) {
-			vpe_sub_ = nh.subscribe("mavros/vision_pose/pose", 1, &OpticalFlow::vpeCallback, this);
-		}
-
 		dyn_srv_ = std::make_shared<dynamic_reconfigure::Server<clover::FlowConfig>>(nh_priv);
 		dynamic_reconfigure::Server<clover::FlowConfig>::CallbackType cb;
 
@@ -130,12 +121,6 @@ private:
 	{
 		if (!enabled_) return;
 
-		if (disable_on_vpe_ &&
-			!last_vpe_time_.isZero() &&
-			(msg->header.stamp - last_vpe_time_).toSec() < 0.1) {
-			return;
-		}
-
 		parseCameraInfo(cinfo);
 
 		auto img = cv_bridge::toCvShare(msg, "mono8")->image;
@@ -251,14 +236,6 @@ private:
 			prev_ = curr_.clone();
 			prev_stamp_ = msg->header.stamp;
 
-			// Publish estimated angular velocity
-			geometry_msgs::TwistStamped velo;
-			velo.header.stamp = msg->header.stamp;
-			velo.header.frame_id = fcu_frame_id_;
-			velo.twist.angular.x = flow_fcu.vector.x / integration_time.toSec();
-			velo.twist.angular.y = flow_fcu.vector.y / integration_time.toSec();
-			velo_pub_.publish(velo);
-
 publish_debug:
 			// Publish debug image
 			if (img_pub_.getNumSubscribers() > 0) {
@@ -271,6 +248,14 @@ publish_debug:
 				out_msg.image = img;
 				img_pub_.publish(out_msg.toImageMsg());
 			}
+
+			// Publish estimated angular velocity
+			geometry_msgs::TwistStamped velo;
+			velo.header.stamp = msg->header.stamp;
+			velo.header.frame_id = fcu_frame_id_;
+			velo.twist.angular.x = flow_fcu.vector.x / integration_time.toSec();
+			velo.twist.angular.y = flow_fcu.vector.y / integration_time.toSec();
+			velo_pub_.publish(velo);
 		}
 	}
 
@@ -299,10 +284,6 @@ publish_debug:
 			prev_ = Mat(); // clear previous frame
 		}
 	}
-
-	void vpeCallback(const geometry_msgs::PoseStamped& vpe) {
-		last_vpe_time_ = vpe.header.stamp;
-	}
 };
 
 PLUGINLIB_EXPORT_CLASS(OpticalFlow, nodelet::Nodelet)

clover/src/selfcheck.py

@@ -15,8 +15,7 @@ import subprocess
 import re
 from collections import OrderedDict
 import traceback
-import threading
+from threading import Event
-from threading import Event, Thread, Lock
 import numpy
 import rospy
 import tf2_ros
@@ -28,16 +27,24 @@ from mavros_msgs.msg import State, OpticalFlowRad, Mavlink
 from mavros_msgs.srv import ParamGet
 from geometry_msgs.msg import PoseStamped, TwistStamped, PoseWithCovarianceStamped, Vector3Stamped
 from visualization_msgs.msg import MarkerArray as VisualizationMarkerArray
-from diagnostic_msgs.msg import DiagnosticArray
 import tf.transformations as t
 from aruco_pose.msg import MarkerArray
 from mavros import mavlink
 import locale
 
 
+# TODO: check attitude is present
+# TODO: disk free space
+# TODO: map, base_link, body
+# TODO: rc service
+# 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')
 
-os.environ['ROSCONSOLE_FORMAT']='${message}'
+os.environ['ROSCONSOLE_FORMAT']='[${severity}]: ${message}'
 
 # use user's locale to convert numbers, etc
 locale.setlocale(locale.LC_ALL, '')
@@ -46,58 +53,46 @@ tf_buffer = tf2_ros.Buffer()
 tf_listener = tf2_ros.TransformListener(tf_buffer)
 
 
-thread_local = threading.local()
+failures = []
-reports_lock = Lock()
+infos = []
+current_check = None
 
 
 def failure(text, *args):
     msg = text % args
-    thread_local.reports += [{'failure': msg}]
+    rospy.logwarn('%s: %s', current_check, msg)
+    failures.append(msg)
 
 
 def info(text, *args):
     msg = text % args
-    thread_local.reports += [{'info': msg}]
+    rospy.loginfo('%s: %s', current_check, msg)
+    infos.append(msg)
 
 
 def check(name):
     def inner(fn):
         def wrapper(*args, **kwargs):
-            start = rospy.get_time()
+            failures[:] = []
-            thread_local.reports = []
+            infos[:] = []
+            global current_check
+            current_check = name
             try:
                 fn(*args, **kwargs)
             except Exception as e:
                 traceback.print_exc()
                 rospy.logerr('%s: exception occurred', name)
-            with reports_lock:
+                return
-                for report in thread_local.reports:
+            if not failures and not infos:
-                    if 'failure' in report:
+                rospy.loginfo('%s: OK', name)
-                        rospy.logerr('%s: %s', name, report['failure'])
-                    elif 'info' in report:
-                        rospy.loginfo('\033[90m%s\033[0m: %s', name, report['info'])
-                if not thread_local.reports:
-                    rospy.loginfo('\033[90m%s\033[0m: \033[92mOK\033[0m', name)
-                if rospy.get_param('~time', False):
-                    rospy.loginfo('%s: %.1f sec', name, rospy.get_time() - start)
         return wrapper
     return inner
 
 
-def ff(value, precision=2):
-    # safely format float or int
-    if value is None:
-        return '\033[31m???\033[0m'
-    if isinstance(value, float):
-        return ('{:.' + str(precision + 1) + '}').format(value)
-    elif isinstance(value, int):
-        return str(value)
-
-
 param_get = rospy.ServiceProxy('mavros/param/get', ParamGet)
 
 
-def get_param(name, default=None):
+def get_param(name):
     try:
         res = param_get(param_id=name)
     except rospy.ServiceException as e:
@@ -106,17 +101,12 @@ def get_param(name, default=None):
 
     if not res.success:
         failure('unable to retrieve PX4 parameter %s', name)
-        return default
     else:
         if res.value.integer != 0:
             return res.value.integer
         return res.value.real
 
 
-def get_paramf(name, precision=2):
-    return ff(get_param(name), precision)
-
-
 recv_event = Event()
 link = mavutil.mavlink.MAVLink('', 255, 1)
 mavlink_pub = rospy.Publisher('mavlink/to', Mavlink, queue_size=1)
@@ -161,24 +151,6 @@ def mavlink_exec(cmd, timeout=3.0):
     return mavlink_recv
 
 
-def read_diagnostics(name, key):
-    e = Event()
-    def cb(msg):
-        for status in msg.status:
-            if status.name.lower() == name.lower():
-                for value in status.values:
-                    if value.key.lower() == key.lower():
-                        cb.value = value.value
-                        e.set()
-                        return
-
-    cb.value = None
-    sub = rospy.Subscriber('/diagnostics', DiagnosticArray, cb)
-    e.wait(1.0)  # wait to read all the diagnostics from nodes publishing them
-    sub.unregister()
-    return cb.value
-
-
 BOARD_ROTATIONS = {
     0: 'no rotation',
     1: 'yaw 45°',
@@ -226,28 +198,27 @@ def check_fcu():
             failure('no connection to the FCU (check wiring)')
             return
 
-        if not is_process_running('px4', exact=True): # can't use px4 console in SITL
+        clover_tag = re.compile(r'-cl[oe]ver\.\d+$')
-            clover_tag = re.compile(r'-cl[oe]ver\.\d+$')
+        clover_fw = False
-            clover_fw = False
 
-            # Make sure the console is available to us
+        # Make sure the console is available to us
-            mavlink_exec('\n')
+        mavlink_exec('\n')
-            version_str = mavlink_exec('ver all')
+        version_str = mavlink_exec('ver all')
-            if version_str == '':
+        if version_str == '':
-                info('no version data available from SITL')
+            info('no version data available from SITL')
 
-            for line in version_str.split('\n'):
+        for line in version_str.split('\n'):
-                if line.startswith('FW version: '):
+            if line.startswith('FW version: '):
-                    info(line[len('FW version: '):])
+                info(line[len('FW version: '):])
-                elif line.startswith('FW git tag: '): # only Clover's firmware
+            elif line.startswith('FW git tag: '): # only Clover's firmware
-                    tag = line[len('FW git tag: '):]
+                tag = line[len('FW git tag: '):]
-                    clover_fw = clover_tag.search(tag)
+                clover_fw = clover_tag.search(tag)
-                    info(tag)
+                info(tag)
-                elif line.startswith('HW arch: '):
+            elif line.startswith('HW arch: '):
-                    info(line[len('HW arch: '):])
+                info(line[len('HW arch: '):])
 
-            if not clover_fw:
+        if not clover_fw:
-                info('not Clover PX4 firmware, check https://clover.coex.tech/firmware')
+            info('not Clover PX4 firmware, check https://clover.coex.tech/firmware')
 
         est = get_param('SYS_MC_EST_GROUP')
         if est == 1:
@@ -284,25 +255,18 @@ def check_fcu():
         if cbrk_usb_chk != 197848:
             failure('set parameter CBRK_USB_CHK to 197848 for flying with USB connected')
 
-        if not is_process_running('px4', exact=True): # skip battery check in SITL
-            try:
-                battery = rospy.wait_for_message('mavros/battery', BatteryState, timeout=3)
-                if not battery.cell_voltage:
-                    failure('cell voltage is not available, https://clover.coex.tech/power')
-                else:
-                    cell = battery.cell_voltage[0]
-                    if cell > 4.3 or cell < 3.0:
-                        failure('incorrect cell voltage: %.2f V, https://clover.coex.tech/power', cell)
-                    elif cell < 3.7:
-                        failure('critically low cell voltage: %.2f V, recharge battery', cell)
-            except rospy.ROSException:
-                failure('no battery state')
-
-        # time sync check
         try:
-            info('time sync offset: %.2f s', float(read_diagnostics('mavros: Time Sync', 'Estimated time offset (s)')))
+            battery = rospy.wait_for_message('mavros/battery', BatteryState, timeout=3)
-        except:
+            if not battery.cell_voltage:
-            failure('cannot read time sync offset')
+                failure('cell voltage is not available, https://clover.coex.tech/power')
+            else:
+                cell = battery.cell_voltage[0]
+                if cell > 4.3 or cell < 3.0:
+                    failure('incorrect cell voltage: %.2f V, https://clover.coex.tech/power', cell)
+                elif cell < 3.7:
+                    failure('critically low cell voltage: %.2f V, recharge battery', cell)
+        except rospy.ROSException:
+            failure('no battery state')
 
     except rospy.ROSException:
         failure('no MAVROS state (check wiring)')
@@ -382,8 +346,6 @@ def is_process_running(binary, exact=False, full=False):
 
 @check('ArUco markers')
 def check_aruco():
-    markers = None
-
     if is_process_running('aruco_detect', full=True):
         try:
             info('aruco_detect/length = %g m', rospy.get_param('aruco_detect/length'))
@@ -394,9 +356,9 @@ def check_aruco():
             known_tilt += ' (ALL markers are on the floor)'
         elif known_tilt == 'map_flipped':
             known_tilt += ' (ALL markers are on the ceiling)'
-        info('aruco_detect/known_tilt = %s', known_tilt)
+        info('aruco_detector/known_tilt = %s', known_tilt)
         try:
-            markers = rospy.wait_for_message('aruco_detect/markers', MarkerArray, timeout=0.8)
+            rospy.wait_for_message('aruco_detect/markers', MarkerArray, timeout=1)
         except rospy.ROSException:
             failure('no markers detection')
             return
@@ -413,49 +375,34 @@ def check_aruco():
         info('aruco_map/known_tilt = %s', known_tilt)
 
         try:
-            visualization = rospy.wait_for_message('aruco_map/visualization', VisualizationMarkerArray, timeout=0.8)
+            visualization = rospy.wait_for_message('aruco_map/visualization', VisualizationMarkerArray, timeout=1)
             info('map has %s markers', len(visualization.markers))
         except:
             failure('cannot read aruco_map/visualization topic')
 
         try:
-            rospy.wait_for_message('aruco_map/pose', PoseWithCovarianceStamped, timeout=0.8)
+            rospy.wait_for_message('aruco_map/pose', PoseWithCovarianceStamped, timeout=1)
         except rospy.ROSException:
-            if not markers:
+            failure('no map detection')
-                info('no map detection as no markers detection')
-            elif not markers.markers:
-                info('no map detection as no markers detected')
-            else:
-                failure('no map detection')
     else:
         info('aruco_map is not running')
 
 
-def is_on_the_floor():
-    try:
-        dist = rospy.wait_for_message('rangefinder/range', Range, timeout=1)
-        return dist.range < 0.3
-    except rospy.ROSException:
-        return False
-
-
 @check('Vision position estimate')
 def check_vpe():
     vis = None
     try:
-        vis = rospy.wait_for_message('mavros/vision_pose/pose', PoseStamped, timeout=0.8)
+        vis = rospy.wait_for_message('mavros/vision_pose/pose', PoseStamped, timeout=1)
     except rospy.ROSException:
         try:
-            vis = rospy.wait_for_message('mavros/mocap/pose', PoseStamped, timeout=0.8)
+            vis = rospy.wait_for_message('mavros/mocap/pose', PoseStamped, timeout=1)
         except rospy.ROSException:
-            if not is_process_running('vpe_publisher', full=True):
+            failure('no VPE or MoCap messages')
-                info('no vision position estimate, vpe_publisher is not running')
+            # check if vpe_publisher is running
-            elif rospy.get_param('aruco_map/known_tilt') == 'map_flipped':
+            try:
-                failure('no vision position estimate, markers are on the ceiling')
+                subprocess.check_output(['pgrep', '-x', 'vpe_publisher'])
-            elif is_on_the_floor():
+            except subprocess.CalledProcessError:
-                info('no vision position estimate, the drone is on the floor')
+                return  # it's not running, skip following checks
-            else:
-                failure('no vision position estimate')
 
     # check PX4 settings
     est = get_param('SYS_MC_EST_GROUP')
@@ -467,14 +414,14 @@ def check_vpe():
         if vision_yaw_w == 0:
             failure('vision yaw weight is zero, change ATT_W_EXT_HDG parameter')
         else:
-            info('vision yaw weight: %s', ff(vision_yaw_w))
+            info('Vision yaw weight: %.2f', vision_yaw_w)
         fuse = get_param('LPE_FUSION')
         if not fuse & (1 << 2):
             failure('vision position fusion is disabled, change LPE_FUSION parameter')
         delay = get_param('LPE_VIS_DELAY')
         if delay != 0:
-            failure('LPE_VIS_DELAY = %s, but it should be zero', delay)
+            failure('LPE_VIS_DELAY parameter is %s, but it should be zero', delay)
-        info('LPE_VIS_XY = %s m, LPE_VIS_Z = %s m', get_paramf('LPE_VIS_XY'), get_paramf('LPE_VIS_Z'))
+        info('LPE_VIS_XY is %.2f m, LPE_VIS_Z is %.2f m', get_param('LPE_VIS_XY'), get_param('LPE_VIS_Z'))
     elif est == 2:
         fuse = get_param('EKF2_AID_MASK')
         if not fuse & (1 << 3):
@@ -483,10 +430,10 @@ def check_vpe():
             failure('vision yaw fusion is disabled, change EKF2_AID_MASK parameter')
         delay = get_param('EKF2_EV_DELAY')
         if delay != 0:
-            failure('EKF2_EV_DELAY = %.2f, but it should be zero', delay)
+            failure('EKF2_EV_DELAY is %.2f, but it should be zero', delay)
-        info('EKF2_EVA_NOISE = %s, EKF2_EVP_NOISE = %s',
+        info('EKF2_EVA_NOISE is %.3f, EKF2_EVP_NOISE is %.3f',
-            get_paramf('EKF2_EVA_NOISE', 3),
+            get_param('EKF2_EVA_NOISE'),
-            get_paramf('EKF2_EVP_NOISE', 3))
+            get_param('EKF2_EVP_NOISE'))
 
     if not vis:
         return
@@ -584,10 +531,10 @@ def check_velocity():
 @check('Global position (GPS)')
 def check_global_position():
     try:
-        rospy.wait_for_message('mavros/global_position/global', NavSatFix, timeout=0.8)
+        rospy.wait_for_message('mavros/global_position/global', NavSatFix, timeout=1)
     except rospy.ROSException:
         info('no global position')
-        if get_param('SYS_MC_EST_GROUP') == 2 and (get_param('EKF2_AID_MASK', 0) & (1 << 0)):
+        if get_param('SYS_MC_EST_GROUP') == 2 and (get_param('EKF2_AID_MASK') & (1 << 0)):
             failure('enabled GPS fusion may suppress vision position aiding')
 
 
@@ -600,7 +547,7 @@ def check_optical_flow():
         # check PX4 settings
         rot = get_param('SENS_FLOW_ROT')
         if rot != 0:
-            failure('SENS_FLOW_ROT = %s, but it should be zero', rot)
+            failure('SENS_FLOW_ROT parameter is %s, but it should be zero', rot)
         est = get_param('SYS_MC_EST_GROUP')
         if est == 1:
             fuse = get_param('LPE_FUSION')
@@ -608,36 +555,32 @@ def check_optical_flow():
                 failure('optical flow fusion is disabled, change LPE_FUSION parameter')
             if not fuse & (1 << 1):
                 failure('flow gyro compensation is disabled, change LPE_FUSION parameter')
-            scale = get_param('LPE_FLW_SCALE', 1)
+            scale = get_param('LPE_FLW_SCALE')
             if not numpy.isclose(scale, 1.0):
-                failure('LPE_FLW_SCALE = %.2f, but it should be 1.0', scale)
+                failure('LPE_FLW_SCALE parameter is %.2f, but it should be 1.0', scale)
 
-            info('LPE_FLW_QMIN = %s, LPE_FLW_R = %s, LPE_FLW_RR = %s',
+            info('LPE_FLW_QMIN is %s, LPE_FLW_R is %.4f, LPE_FLW_RR is %.4f, SENS_FLOW_MINHGT is %.3f, SENS_FLOW_MAXHGT is %.3f',
-                          get_paramf('LPE_FLW_QMIN'),
+                          get_param('LPE_FLW_QMIN'),
-                          get_paramf('LPE_FLW_R', 4),
+                          get_param('LPE_FLW_R'),
-                          get_paramf('LPE_FLW_RR', 4))
+                          get_param('LPE_FLW_RR'),
+                          get_param('SENS_FLOW_MINHGT'),
+                          get_param('SENS_FLOW_MAXHGT'))
         elif est == 2:
-            fuse = get_param('EKF2_AID_MASK', 0)
+            fuse = get_param('EKF2_AID_MASK')
             if not fuse & (1 << 1):
                 failure('optical flow fusion is disabled, change EKF2_AID_MASK parameter')
-            delay = get_param('EKF2_OF_DELAY', 0)
+            delay = get_param('EKF2_OF_DELAY')
             if delay != 0:
-                failure('EKF2_OF_DELAY = %.2f, but it should be zero', delay)
+                failure('EKF2_OF_DELAY is %.2f, but it should be zero', delay)
-            info('EKF2_OF_QMIN = %s, EKF2_OF_N_MIN = %s, EKF2_OF_N_MAX = %s',
+            info('EKF2_OF_QMIN is %s, EKF2_OF_N_MIN is %.4f, EKF2_OF_N_MAX is %.4f, SENS_FLOW_MINHGT is %.3f, SENS_FLOW_MAXHGT is %.3f',
-                          get_paramf('EKF2_OF_QMIN'),
+                          get_param('EKF2_OF_QMIN'),
-                          get_paramf('EKF2_OF_N_MIN', 4),
+                          get_param('EKF2_OF_N_MIN'),
-                          get_paramf('EKF2_OF_N_MAX', 4))
+                          get_param('EKF2_OF_N_MAX'),
-        info('SENS_FLOW_MINHGT = %s, SENS_FLOW_MAXHGT = %s', get_paramf('SENS_FLOW_MINHGT', 3), get_paramf('SENS_FLOW_MAXHGT', 3))
+                          get_param('SENS_FLOW_MINHGT'),
+                          get_param('SENS_FLOW_MAXHGT'))
 
     except rospy.ROSException:
-        if rospy.get_param('optical_flow/disable_on_vpe', False):
+        failure('no optical flow data (from Raspberry)')
-            try:
-                rospy.wait_for_message('mavros/vision_pose/pose', PoseStamped, timeout=1)
-                info('no optical flow as disable_on_vpe is true')
-            except:
-                failure('no optical flow on RPi, disable_on_vpe is true, but no vision pose also')
-        else:
-            failure('no optical flow on RPi')
 
 
 @check('Rangefinder')
@@ -661,7 +604,7 @@ def check_rangefinder():
 
     est = get_param('SYS_MC_EST_GROUP')
     if est == 1:
-        fuse = get_param('LPE_FUSION', 0)
+        fuse = get_param('LPE_FUSION')
         if not fuse & (1 << 5):
             info('"pub agl as lpos down" in LPE_FUSION is disabled, NOT operating over flat surface')
         else:
@@ -695,7 +638,7 @@ def check_boot_duration():
 
 @check('CPU usage')
 def check_cpu_usage():
-    WHITELIST = 'nodelet', 'gzclient', 'gzserver', 'selfcheck.py'
+    WHITELIST = 'nodelet', 'gzclient', 'gzserver'
     CMD = "top -n 1 -b -i | tail -n +8 | awk '{ printf(\"%-8s\\t%-8s\\t%-8s\\n\", $1, $9, $12); }'"
     output = subprocess.check_output(CMD, shell=True).decode()
     processes = output.split('\n')
@@ -764,10 +707,7 @@ def check_image():
     try:
         info('version: %s', open('/etc/clover_version').read().strip())
     except IOError:
-        try:
+        info('no /etc/clover_version file, not the Clover image?')
-            info('VM version: %s', open('/etc/clover_vm_version').read().strip())
-        except IOError:
-            info('no /etc/clover_version file, not the Clover image?')
 
 
 @check('Preflight status')
@@ -878,47 +818,26 @@ def check_board():
         info('could not open /proc/device-tree/model, not a Raspberry Pi?')
 
 
-def parallel_for(fns):
-    threads = []
-    for fn in fns:
-        thread = Thread(target=fn)
-        thread.start()
-        threads.append(thread)
-    for thread in threads:
-        thread.join()
-
-
-def consequentially_for(fns):
-    for fn in fns:
-        fn()
-
-
 def selfcheck():
-    checks = [
+    check_image()
-        check_image,
+    check_board()
-        check_board,
+    check_clover_service()
-        check_clover_service,
+    check_network()
-        check_network,
+    check_fcu()
-        check_fcu,
+    check_imu()
-        check_imu,
+    check_local_position()
-        check_local_position,
+    check_velocity()
-        check_velocity,
+    check_global_position()
-        check_global_position,
+    check_preflight_status()
-        check_preflight_status,
+    check_main_camera()
-        check_main_camera,
+    check_aruco()
-        check_aruco,
+    check_simpleoffboard()
-        check_simpleoffboard,
+    check_optical_flow()
-        check_optical_flow,
+    check_vpe()
-        check_vpe,
+    check_rangefinder()
-        check_rangefinder,
+    check_rpi_health()
-        check_rpi_health,
+    check_cpu_usage()
-        check_cpu_usage,
+    check_boot_duration()
-        check_boot_duration,
-    ]
-    if rospy.get_param('~parallel', False):
-        parallel_for(checks)
-    else:
-        consequentially_for(checks)
 
 
 if __name__ == '__main__':

clover/src/simple_offboard.cpp

@@ -78,6 +78,7 @@ ros::Duration manual_control_timeout;
 float default_speed;
 bool auto_release;
 bool land_only_in_offboard, nav_from_sp, check_kill_switch;
+bool nav_feedforward;
 std::map<string, string> reference_frames;
 
 // Publishers
@@ -94,6 +95,7 @@ PositionTarget position_raw_msg;
 AttitudeTarget att_raw_msg;
 Thrust thrust_msg;
 TwistStamped rates_msg;
+Vector3 velocity_feedforward;
 TransformStamped target, setpoint;
 geometry_msgs::TransformStamped body;
 
@@ -341,7 +343,15 @@ inline float getDistance(const Point& from, const Point& to)
 	return hypot(from.x - to.x, from.y - to.y, from.z - to.z);
 }
 
-void getNavigateSetpoint(const ros::Time& stamp, float speed, Point& nav_setpoint)
+inline void normalize(Vector3& vector, double length = 1.0)
+{
+	double len = hypot(vector.x, vector.y, vector.z);
+	vector.x *= length / len;
+	vector.y *= length / len;
+	vector.z *= length / len;
+}
+
+void getNavigateSetpoint(const ros::Time& stamp, float speed, Point& nav_setpoint, Vector3& velocity_feedforward)
 {
 	if (wait_armed) {
 		// don't start navigating if we're waiting arming
@@ -355,6 +365,14 @@ void getNavigateSetpoint(const ros::Time& stamp, float speed, Point& nav_setpoin
 	nav_setpoint.x = nav_start.pose.position.x + (setpoint_position_transformed.pose.position.x - nav_start.pose.position.x) * passed;
 	nav_setpoint.y = nav_start.pose.position.y + (setpoint_position_transformed.pose.position.y - nav_start.pose.position.y) * passed;
 	nav_setpoint.z = nav_start.pose.position.z + (setpoint_position_transformed.pose.position.z - nav_start.pose.position.z) * passed;
+
+	if (nav_feedforward) {
+		// calculate velocity feedforward
+		velocity_feedforward.x = setpoint_position_transformed.pose.position.x - nav_start.pose.position.x;
+		velocity_feedforward.y = setpoint_position_transformed.pose.position.y - nav_start.pose.position.y;
+		velocity_feedforward.z = setpoint_position_transformed.pose.position.z - nav_start.pose.position.z;
+		normalize(velocity_feedforward, speed);
+	}
 }
 
 PoseStamped globalToLocal(double lat, double lon)
@@ -412,7 +430,7 @@ void publish(const ros::Time stamp)
 
 	if (setpoint_type == NAVIGATE || setpoint_type == NAVIGATE_GLOBAL) {
 		position_msg.pose.orientation = setpoint_position_transformed.pose.orientation; // copy yaw
-		getNavigateSetpoint(stamp, nav_speed, position_msg.pose.position);
+		getNavigateSetpoint(stamp, nav_speed, position_msg.pose.position, velocity_feedforward);
 
 		if (setpoint_yaw_type == TOWARDS) {
 			double yaw_towards = atan2(position_msg.pose.position.y - nav_start.pose.position.y,
@@ -428,19 +446,31 @@ void publish(const ros::Time stamp)
 	if (setpoint_type == POSITION || setpoint_type == NAVIGATE || setpoint_type == NAVIGATE_GLOBAL) {
 		position_msg.header.stamp = stamp;
 
-		if (setpoint_yaw_type == YAW || setpoint_yaw_type == TOWARDS) {
+		if (!nav_feedforward && setpoint_yaw_type == YAW || setpoint_yaw_type == TOWARDS) {
+			// simple pose setpoint without velocity feedforward and yaw rate
 			position_pub.publish(position_msg);
 
 		} else {
-			position_raw_msg.type_mask = PositionTarget::IGNORE_VX +
+			position_raw_msg.type_mask = PositionTarget::IGNORE_AFX +
-			                             PositionTarget::IGNORE_VY +
-			                             PositionTarget::IGNORE_VZ +
-			                             PositionTarget::IGNORE_AFX +
 			                             PositionTarget::IGNORE_AFY +
-			                             PositionTarget::IGNORE_AFZ +
+			                             PositionTarget::IGNORE_AFZ;
-			                             PositionTarget::IGNORE_YAW;
+
+			if (!nav_feedforward) {
+				position_raw_msg.type_mask = PositionTarget::IGNORE_VX +
+				                             PositionTarget::IGNORE_VY +
+				                             PositionTarget::IGNORE_VZ;
+
+			}
+
+			position_raw_msg.type_mask += setpoint_yaw_type == YAW_RATE ?
+				PositionTarget::IGNORE_YAW : PositionTarget::IGNORE_YAW_RATE;
+
 			position_raw_msg.yaw_rate = setpoint_yaw_rate;
+			position_raw_msg.yaw = setpoint_yaw;
 			position_raw_msg.position = position_msg.pose.position;
+			position_raw_msg.velocity.x = velocity_feedforward.x;
+			position_raw_msg.velocity.y = velocity_feedforward.y;
+			position_raw_msg.velocity.z = velocity_feedforward.z;
 			position_raw_pub.publish(position_raw_msg);
 		}
 
@@ -886,6 +916,7 @@ int main(int argc, char **argv)
 	nh_priv.param("land_only_in_offboard", land_only_in_offboard, true);
 	nh_priv.param("nav_from_sp", nav_from_sp, true);
 	nh_priv.param("check_kill_switch", check_kill_switch, true);
+	nh_priv.param("nav_feedforward", nav_feedforward, true);
 	nh_priv.param("default_speed", default_speed, 0.5f);
 	nh_priv.param<string>("body_frame", body.child_frame_id, "body");
 	nh_priv.getParam("reference_frames", reference_frames);

clover/test/basic.py

@@ -3,7 +3,6 @@ import rospy
 import pytest
 from mavros_msgs.msg import State
 from clover import srv
-import time
 
 @pytest.fixture()
 def node():
@@ -61,18 +60,3 @@ def test_blocks(node):
 
     t.join()
     assert wait_print.result == 'test'
-
-def test_long_callback():
-    from clover import long_callback
-    from time import sleep
-
-    # very basic test for long_callback
-    @long_callback
-    def cb(i):
-        cb.counter += i
-    cb.counter = 0
-    cb(2)
-    sleep(0.1)
-    cb(3)
-    sleep(1)
-    assert cb.counter == 5

clover/www/index.html

@@ -12,10 +12,6 @@
 	<li><a href="console.html">Clover console</a> (<code>/var/log/clover.log</code>)</li>
 </ul>
 
-<p>
-	Update www using <code>rosrun roswww_static update</code>.
-</p>
-
 <div class="version"></div>
 
 <script type="text/javascript">

clover_description/urdf/sensors/distance_sensor.urdf.xacro

@@ -2,7 +2,7 @@
 <robot name="rpi_camera" xmlns:xacro="http://ros.org/wiki/xacro">
   <xacro:include filename="../camera_sensor.urdf.xacro"/>
 
-  <xacro:macro name="distance_sensor" params="name:=lidar_vl53l1x parent x:=0 y:=0 z:=0 roll:=0 pitch:=0 yaw:=0 range_min:=0.01 range_max:=4.0 resolution:=0.01 rate:=10 fov:=0.471239">
+  <xacro:macro name="distance_sensor" params="name:=lidar_vl53l1x parent x:=0 y:=0 z:=0 roll:=0 pitch:=0 yaw:=0 range_min:=0.01 range_max:=4.0 resolution:=0.01 rate:=10">
     <joint name="${name}_joint" type="fixed">
       <origin xyz="${x} ${y} ${z}"
               rpy="${roll} ${pitch} ${yaw}"/>
@@ -58,7 +58,7 @@
           <topicName>/rangefinder/range</topicName>
           <frameName>rangefinder</frameName>
           <radiation>infrared</radiation>
-          <fov>${fov}</fov>
+          <fov>0.01</fov>
           <gaussianNoise>0.001</gaussianNoise>
           <updateRate>${rate}</updateRate>
           <min_distance>${range_min}</min_distance>

clover_simulation/src/sim_leds.cpp

@@ -65,8 +65,7 @@ public:
 		}
 
 		role = (ros::this_node::getName() == "/gazebo") ? Role::Server : Role::Client;
-		ROS_INFO_NAMED(("LedController_" + robotNamespace).c_str(), "LedController has started (as %s) in namespace '%s'",
+		ROS_INFO_NAMED(("LedController_" + robotNamespace).c_str(), "LedController has started (as %s)", role == Role::Client ? "client" : "server");
-			role == Role::Client ? "client" : "server", robotNamespace.c_str());
 
 		nh.reset(new ros::NodeHandle(robotNamespace));
 
@@ -110,6 +109,7 @@ LedController& get(std::string robotNamespace)
 	std::lock_guard<std::mutex> lock(controllerMutex);
 	auto it = controllers.find(robotNamespace);
 	if (it == controllers.end()) {
+		gzwarn << "Creating new LED controller for namespace " << robotNamespace << "\n";
 		controllers[robotNamespace].reset(new LedController(robotNamespace));
 		return *controllers[robotNamespace];
 	}

docs/en/educational_contests.md

@@ -20,7 +20,7 @@ The main goal of the contest is aerial robotics popularization  and community de
 * Third parties can provide technical support for recording a lecture.
 * The status of the participant is unlimited (student, representative of a general education institution, representative of the industry, amateur).
 
-Applications deadline: November 30, 2022.
+Applications deadline: September 1, 2022.
 
 ### How to apply?
 
@@ -64,7 +64,7 @@ The main goal of the contest is aerial robotics popularization  and community de
 
 The application to the contest is performed via the [Google Form](https://docs.google.com/forms/d/e/1FAIpQLSdelVy6yQ1iN6u88KeiEIKGj7gGaM0xccSt2tiYKB46ICmjkQ/viewform).
 
-Applications deadline: November 30, 2022.
+Applications deadline: September 1, 2022.
 
 ### Prizes
 
@@ -105,7 +105,7 @@ The course is evaluated according to a separate, publicly available lesson submi
 
 The application to the contest is performed via the [Google Form](https://docs.google.com/forms/d/e/1FAIpQLSdf2Q68X4hPnFE9f3EP95AxPNnzHKqIsFHtTRT6EBKiH93wzg/viewform) where the link to the video course should be attached.
 
-Applications deadline: November 30, 2022.
+Applications deadline: September 1, 2022.
 
 ### Prizes
 

docs/en/snippets.md

@@ -207,9 +207,9 @@ def pose_update(pose):
     # Processing new data of copter's position
     pass
 
-rospy.Subscriber('mavros/local_position/pose', PoseStamped, pose_update)
+rospy.Subscriber('/mavros/local_position/pose', PoseStamped, pose_update)
-rospy.Subscriber('mavros/local_position/velocity', TwistStamped, velocity_update)
+rospy.Subscriber('/mavros/local_position/velocity', TwistStamped, velocity_update)
-rospy.Subscriber('mavros/battery', BatteryState, battery_update)
+rospy.Subscriber('/mavros/battery', BatteryState, battery_update)
 rospy.Subscriber('mavros/rc/in', RCIn, rc_callback)
 
 rospy.spin()
@@ -349,7 +349,7 @@ from pymavlink import mavutil
 from mavros_msgs.srv import CommandLong
 from mavros_msgs.msg import State
 
-send_command = rospy.ServiceProxy('mavros/cmd/command', CommandLong)
+send_command = rospy.ServiceProxy('/mavros/cmd/command', CommandLong)
 
 def calibrate_gyro():
     rospy.loginfo('Calibrate gyro')
@@ -480,11 +480,3 @@ param_set(param_id='COM_FLTMODE1', value=ParamValue(integer=8))
 # Set parameter of type FLOAT:
 param_set(param_id='MPC_Z_P', value=ParamValue(real=1.5))
 ```
-
-### # {#is-simulation}
-
-Check, if the code is running inside a [Gazebo simulation](simulation.md):
-
-```python
-is_simulation = rospy.get_param('/use_sim_time', False)
-```

docs/ru/educational_contests.md

@@ -26,7 +26,7 @@
 
 Прием заявок осуществляется через [Google Форму](https://docs.google.com/forms/d/e/1FAIpQLScE2kN5dO2OYNSM8hOYzOa5Qvh2uDdd9Fjx8OnL1W93bfEBgw/viewform).
 
-Дедлайн подачи заявок: 30 ноября 2022 года.
+Дедлайн подачи заявок: 1 сентября 2022 года.
 
 ### Призы
 
@@ -64,7 +64,7 @@
 
 Прием заявок осуществляется через [Google Форму](https://docs.google.com/forms/d/e/1FAIpQLSdelVy6yQ1iN6u88KeiEIKGj7gGaM0xccSt2tiYKB46ICmjkQ/viewform).
 
-Дедлайн подачи заявок: 30 ноября 2022 года.
+Дедлайн подачи заявок: 1 сентября 2022 года.
 
 ### Призы
 
@@ -106,7 +106,7 @@
 
 Прием заявок осуществляется через [Google Форму](https://docs.google.com/forms/d/e/1FAIpQLSdf2Q68X4hPnFE9f3EP95AxPNnzHKqIsFHtTRT6EBKiH93wzg/viewform).
 
-Дедлайн подачи заявок: 30 ноября 2022 года
+Дедлайн подачи заявок: 1 сентября 2022 года
 
 ### Призы
 

docs/ru/snippets.md

@@ -217,9 +217,9 @@ def pose_update(pose):
     # Обработка новых данных о позиции коптера
     pass
 
-rospy.Subscriber('mavros/local_position/pose', PoseStamped, pose_update)
+rospy.Subscriber('/mavros/local_position/pose', PoseStamped, pose_update)
-rospy.Subscriber('mavros/local_position/velocity', TwistStamped, velocity_update)
+rospy.Subscriber('/mavros/local_position/velocity', TwistStamped, velocity_update)
-rospy.Subscriber('mavros/battery', BatteryState, battery_update)
+rospy.Subscriber('/mavros/battery', BatteryState, battery_update)
 rospy.Subscriber('mavros/rc/in', RCIn, rc_callback)
 
 rospy.spin()
@@ -360,7 +360,7 @@ from pymavlink import mavutil
 from mavros_msgs.srv import CommandLong
 from mavros_msgs.msg import State
 
-send_command = rospy.ServiceProxy('mavros/cmd/command', CommandLong)
+send_command = rospy.ServiceProxy('/mavros/cmd/command', CommandLong)
 
 def calibrate_gyro():
     rospy.loginfo('Calibrate gyro')
@@ -491,11 +491,3 @@ param_set(param_id='COM_FLTMODE1', value=ParamValue(integer=8))
 # Изменить параметр типа FLOAT:
 param_set(param_id='MPC_Z_P', value=ParamValue(real=1.5))
 ```
-
-### # {#is-simulation}
-
-Проверить, что код запущен в [симуляции Gazebo](simulation.md):
-
-```python
-is_simulation = rospy.get_param('/use_sim_time', False)
-```

redirects.json

@@ -35,7 +35,7 @@
         { "from": "snippets.html", "to": "ru/snippets.html" },
         { "from": "camera_frame.html", "to": "ru/camera_setup.html" },
         { "from": "ru/camera_frame.html", "to": "camera_setup.html" },
-        { "from": "camera.html", "to": "en/camera.html" },
+        { "from": "camera.html", "to": "ru/camera.html" },
         { "from": "led.html", "to": "en/leds.html" },
         { "from": "leds.html", "to": "ru/leds.html" },
         { "from": "rviz.html", "to": "ru/rviz.html" },
@@ -51,7 +51,7 @@
         { "from": "firmware/", "to": "en/firmware.html" },
         { "from": "simple_offboard/", "to": "en/simple_offboard.html" },
         { "from": "offboard/", "to": "en/simple_offboard.html" },
-        { "from": "camera/", "to": "en/camera.html" },
+        { "from": "camera/", "to": "ru/camera.html" },
         { "from": "snippets/", "to": "ru/snippets.html" },
         { "from": "optical_flow/", "to": "ru/optical_flow.html" },
         { "from": "laser/", "to": "ru/laser.html" },

roswww_static/CMakeLists.txt

@@ -5,19 +5,8 @@ find_package(catkin REQUIRED)
 
 catkin_package()
 
-macro(roswww_static_make_default)
-  message(STATUS "roswww_static: make ${PROJECT_NAME} package default")
-  set(ROSWWW_STATIC_DEFAULT ${PROJECT_NAME} CACHE STRING "Default roswww_static package")
-endmacro()
-
 install(DIRECTORY launch DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION})
 
-catkin_install_python(PROGRAMS src/update
+catkin_install_python(PROGRAMS main.py
     DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
 )
-
-message(status "CATKIN_ENV: ${CATKIN_ENV}")
-
-add_custom_target(roswww_static ALL
-    COMMAND ${CMAKE_COMMAND} -E env ROSWWW_STATIC_DEFAULT=${ROSWWW_STATIC_DEFAULT}
-    ${CATKIN_ENV} ${CMAKE_CURRENT_SOURCE_DIR}/src/update)

roswww_static/README.md

@@ -6,12 +6,12 @@ Note: you should configure your web server to make it follow symlinks.
 
 ## Instructions
 
-* Run `update` script and it will generate the symlinks and index file: `rosrun roswww_static update`.
+* Run `main.py` node and it will generate the symlinks and index file.
 * Point your static web server path to `~/.ros/www`.
 
-You can rerun `update` if the list of installed packages changes.
+You can rerun `main.py` if the list of installed packages changes.
 
 ## Parameters
 
-* `index` – path for index page, otherwise packages list would be generated.
+* `index` – path for index page, otherwise packages list would be generated.
-* `default_package` – if set then the index page would redirect to this package's page.
+* `default_package` – if set then the index page would redirect to this package's page.

roswww_static/launch/example.launch

@@ -1,5 +1,5 @@
 <launch>
-    <node pkg="roswww_static" name="roswww_static" type="update" clear_params="true" output="screen">
+    <node pkg="roswww_static" name="roswww_static" type="main.py" clear_params="true" output="screen">
         <!-- <param name="index" value="$(find my_package)/www/index.html"/> -->
         <!-- <param name="default_package" value="my_package"/> -->
     </node>

roswww_static/main.py

@@ -1,4 +1,4 @@
-#!/usr/bin/env python3
+#!/usr/bin/env python
 
 # Copyright (C) 2020 Copter Express Technologies
 #
@@ -16,16 +16,13 @@ import shutil
 import rospy
 import rospkg
 
-#rospy.init_node('roswww_static')
+rospy.init_node('roswww_static')
 
 rospack = rospkg.RosPack()
 
 www = rospkg.get_ros_home() + '/www'
-index_file = None # rospy.get_param('~index_file', None)
+index_file = rospy.get_param('~index_file', None)
-default_package = os.environ.get('ROSWWW_STATIC_DEFAULT') # rospy.get_param('~default_package', None)
+default_package = rospy.get_param('~default_package', None)
-
-print('roswww_static: destination directory:', www)
-print('roswww_static: default package:', default_package)
 
 shutil.rmtree(www, ignore_errors=True)  # reset www directory content
 os.mkdir(www)
@@ -37,7 +34,7 @@ index = '<h1>Packages list</h1>\n<ul>\n'
 for name in packages:
     path = rospack.get_path(name)
     if os.path.exists(path + '/www'):
-        print('roswww_static: found www path for package', name)
+        rospy.loginfo('found www path for %s package', name)
         os.symlink(path + '/www', www + '/' + name)
         index += '<li><a href="{name}/">{name}</a></li>'.format(name=name)
 
@@ -45,7 +42,7 @@ if default_package is not None:
     redirect_html = '<meta http-equiv=refresh content="0; url={name}/">'.format(name=default_package)
     open(www + '/index.html', 'w').write(redirect_html)
 elif index_file is not None:
-    print('roswww_static: symlinking index file')
+    rospy.loginfo('symlinking index file')
     os.symlink(index_file, www + '/index.html')
 else:
     open(www + '/index.html', 'w').write(index)