Start working on web pages header

.travis.yml

@@ -73,11 +73,8 @@ jobs:
       node_js:
         - "10"
       before_script:
-        - sudo sh -c "echo ttf-mscorefonts-installer msttcorefonts/accepted-mscorefonts-eula select true | debconf-set-selections"
-        - sudo apt update && sudo apt install -y calibre msttcorefonts
         - npm install gitbook-cli -g
         - npm install markdownlint-cli -g
-        - npm install svgexport -g
         - gitbook -V
         - markdownlint -V
       script:
@@ -86,7 +83,6 @@ jobs:
         - ./check_unused_assets.py
         - gitbook install
         - gitbook build
-        - gitbook pdf ./ _book/clover.pdf
       deploy:
         provider: pages
         local_dir: _book

aruco_pose/CMakeLists.txt

@@ -1,6 +1,8 @@
 cmake_minimum_required(VERSION 3.0)
 project(aruco_pose)
 
+add_definitions(-std=c++11 -Wall -g)
+
 ## Compile as C++11, supported in ROS Kinetic and newer
 add_compile_options(-std=c++11)
 
@@ -23,7 +25,7 @@ find_package(catkin REQUIRED COMPONENTS
 )
 
 find_package(OpenCV 3 REQUIRED COMPONENTS core imgproc calib3d)
-if ("${OpenCV_VERSION_MINOR}" LESS "9")
+if ("${OpenCV_VERSION_MINOR}" LESS "3")
   message(STATUS "OpenCV version too low, using vendored ArUco package")
   include(vendor/VendorOpenCV.cmake)
 else()
@@ -227,5 +229,4 @@ if (CATKIN_ENABLE_TESTING)
   add_rostest(test/test_parser_empty_map.test)
   add_rostest(test/test_node_failure.test)
   add_rostest(test/largemap.test)
-  add_rostest(test/crash_opencv.test)
 endif()

aruco_pose/src/aruco_detect.cpp

@@ -58,9 +58,10 @@ using cv::Mat;
 
 class ArucoDetect : public nodelet::Nodelet {
 private:
-	std::unique_ptr<tf2_ros::TransformBroadcaster> br_;
-	std::unique_ptr<tf2_ros::Buffer> tf_buffer_;
-	std::unique_ptr<tf2_ros::TransformListener> tf_listener_;
+	ros::NodeHandle nh_, nh_priv_;
+	tf2_ros::TransformBroadcaster br_;
+	tf2_ros::Buffer tf_buffer_;
+	tf2_ros::TransformListener tf_listener_{tf_buffer_};
 	std::shared_ptr<dynamic_reconfigure::Server<aruco_pose::DetectorConfig>> dyn_srv_;
 	cv::Ptr<cv::aruco::Dictionary> dictionary_;
 	cv::Ptr<cv::aruco::DetectorParameters> parameters_;
@@ -80,32 +81,30 @@ private:
 public:
 	virtual void onInit()
 	{
-		ros::NodeHandle& nh_ = getNodeHandle();
-		ros::NodeHandle& nh_priv_ = getPrivateNodeHandle();
-
-		br_.reset(new tf2_ros::TransformBroadcaster());
-		tf_buffer_.reset(new tf2_ros::Buffer());
-		tf_listener_.reset(new tf2_ros::TransformListener(*tf_buffer_, nh_));
+		nh_ = getNodeHandle();
+		nh_priv_ = getPrivateNodeHandle();
 
 		int dictionary;
-		dictionary = nh_priv_.param("dictionary", 2);
-		estimate_poses_ = nh_priv_.param("estimate_poses", true);
-		send_tf_ = nh_priv_.param("send_tf", true);
+		nh_priv_.param("dictionary", dictionary, 2);
+		nh_priv_.param("estimate_poses", estimate_poses_, true);
+		nh_priv_.param("send_tf", send_tf_, true);
 		if (estimate_poses_ && !nh_priv_.getParam("length", length_)) {
 			NODELET_FATAL("can't estimate marker's poses as ~length parameter is not defined");
 			ros::shutdown();
 		}
-		readLengthOverride(nh_priv_);
+		readLengthOverride();
 
-		known_tilt_ = nh_priv_.param<std::string>("known_tilt", "");
-		auto_flip_ = nh_priv_.param("auto_flip", false);
+		nh_priv_.param<std::string>("known_tilt", known_tilt_, "");
+		nh_priv_.param("auto_flip", auto_flip_, false);
 
-		frame_id_prefix_ = nh_priv_.param<std::string>("frame_id_prefix", "aruco_");
+		nh_priv_.param<std::string>("frame_id_prefix", frame_id_prefix_, "aruco_");
 
 		camera_matrix_ = cv::Mat::zeros(3, 3, CV_64F);
+		dist_coeffs_ = cv::Mat::zeros(8, 1, CV_64F);
 
 		dictionary_ = cv::aruco::getPredefinedDictionary(static_cast<cv::aruco::PREDEFINED_DICTIONARY_NAME>(dictionary));
 		parameters_ = cv::aruco::DetectorParameters::create();
+		parameters_->cornerRefinementMethod = cv::aruco::CORNER_REFINE_SUBPIX;
 
 		image_transport::ImageTransport it(nh_);
 		image_transport::ImageTransport it_priv(nh_priv_);
@@ -171,8 +170,8 @@ private:
 
 				if (!known_tilt_.empty()) {
 					try {
-						snap_to = tf_buffer_->lookupTransform(msg->header.frame_id, known_tilt_,
-						                                      msg->header.stamp, ros::Duration(0.02));
+						snap_to = tf_buffer_.lookupTransform(msg->header.frame_id, known_tilt_,
+						                                     msg->header.stamp, ros::Duration(0.02));
 					} catch (const tf2::TransformException& e) {
 						NODELET_WARN_THROTTLE(5, "can't snap: %s", e.what());
 					}
@@ -206,7 +205,7 @@ private:
 						if (map_markers_ids_.find(ids[i]) == map_markers_ids_.end()) {
 							transform.transform.rotation = marker.pose.orientation;
 							fillTranslation(transform.transform.translation, tvecs[i]);
-							br_->sendTransform(transform);
+							br_.sendTransform(transform);
 						}
 					}
 				}
@@ -327,10 +326,10 @@ private:
 		return frame_id_prefix_ + std::to_string(id);
 	}
 
-	void readLengthOverride(ros::NodeHandle& nh)
+	void readLengthOverride()
 	{
 		std::map<std::string, double> length_override;
-		nh.getParam("length_override", length_override);
+		nh_priv_.getParam("length_override", length_override);
 		for (auto const& item : length_override) {
 			length_override_[std::stoi(item.first)] = item.second;
 		}

aruco_pose/src/aruco_map.cpp

@@ -58,6 +58,7 @@ typedef message_filters::sync_policies::ExactTime<Image, CameraInfo, MarkerArray
 
 class ArucoMap : public nodelet::Nodelet {
 private:
+	ros::NodeHandle nh_, nh_priv_;
 	ros::Publisher img_pub_, pose_pub_, markers_pub_, vis_markers_pub_;
 	image_transport::Publisher debug_pub_;
 	message_filters::Subscriber<Image> image_sub_;
@@ -82,8 +83,8 @@ private:
 public:
 	virtual void onInit()
 	{
-		ros::NodeHandle &nh_ = getNodeHandle();
-		ros::NodeHandle &nh_priv_ = getPrivateNodeHandle();
+		nh_ = getNodeHandle();
+		nh_priv_ = getPrivateNodeHandle();
 
 		image_transport::ImageTransport it_priv(nh_priv_);
 
@@ -95,18 +96,19 @@ public:
 		board_->dictionary = cv::aruco::getPredefinedDictionary(
 			                 static_cast<cv::aruco::PREDEFINED_DICTIONARY_NAME>(nh_priv_.param("dictionary", 2)));
 		camera_matrix_ = cv::Mat::zeros(3, 3, CV_64F);
+		dist_coeffs_ = cv::Mat::zeros(8, 1, CV_64F);
 
 		std::string type, map;
-		type = nh_priv_.param<std::string>("type", "map");
-		transform_.child_frame_id = nh_priv_.param<std::string>("frame_id", "aruco_map");
-		known_tilt_ = nh_priv_.param<std::string>("known_tilt", "");
-		auto_flip_ = nh_priv_.param("auto_flip", false);
-		image_width_ = nh_priv_.param("image_width" , 2000);
-		image_height_ = nh_priv_.param("image_height", 2000);
-		image_margin_ = nh_priv_.param("image_margin", 200);
-		image_axis_ = nh_priv_.param("image_axis", true);
-		markers_parent_frame_ = nh_priv_.param<std::string>("markers/frame_id", transform_.child_frame_id);
-		markers_frame_ = nh_priv_.param<std::string>("markers/child_frame_id_prefix", "");
+		nh_priv_.param<std::string>("type", type, "map");
+		nh_priv_.param<std::string>("frame_id", transform_.child_frame_id, "aruco_map");
+		nh_priv_.param<std::string>("known_tilt", known_tilt_, "");
+		nh_priv_.param("auto_flip", auto_flip_, false);
+		nh_priv_.param("image_width", image_width_, 2000);
+		nh_priv_.param("image_height", image_height_, 2000);
+		nh_priv_.param("image_margin", image_margin_, 200);
+		nh_priv_.param("image_axis", image_axis_, true);
+		nh_priv_.param<std::string>("markers/frame_id", markers_parent_frame_, transform_.child_frame_id);
+		nh_priv_.param<std::string>("markers/child_frame_id_prefix", markers_frame_, "");
 
 		// createStripLine();
 
@@ -114,7 +116,7 @@ public:
 			param(nh_priv_, "map", map);
 			loadMap(map);
 		} else if (type == "gridboard") {
-			createGridBoard(nh_priv_);
+			createGridBoard();
 		} else {
 			NODELET_FATAL("unknown type: %s", type.c_str());
 			ros::shutdown();
@@ -329,7 +331,7 @@ publish_debug:
 		NODELET_INFO("loading %s complete (%d markers)", filename.c_str(), static_cast<int>(board_->ids.size()));
 	}
 
-	void createGridBoard(ros::NodeHandle& nh)
+	void createGridBoard()
 	{
 		NODELET_INFO("generate gridboard");
 		NODELET_WARN("gridboard maps are deprecated");
@@ -337,15 +339,15 @@ publish_debug:
 		int markers_x, markers_y, first_marker;
 		double markers_side, markers_sep_x, markers_sep_y;
 		std::vector<int> marker_ids;
-		markers_x = nh.param("markers_x", 10);
-		markers_y = nh.param("markers_y", 10);
-		first_marker = nh.param("first_marker", 0);
+		nh_priv_.param<int>("markers_x", markers_x, 10);
+		nh_priv_.param<int>("markers_y", markers_y, 10);
+		nh_priv_.param<int>("first_marker", first_marker, 0);
 
-		param(nh, "markers_side", markers_side);
-		param(nh, "markers_sep_x", markers_sep_x);
-		param(nh, "markers_sep_y", markers_sep_y);
+		param(nh_priv_, "markers_side", markers_side);
+		param(nh_priv_, "markers_sep_x", markers_sep_x);
+		param(nh_priv_, "markers_sep_y", markers_sep_y);
 
-		if (nh.getParam("marker_ids", marker_ids)) {
+		if (nh_priv_.getParam("marker_ids", marker_ids)) {
 			if ((unsigned int)(markers_x * markers_y) != marker_ids.size()) {
 				NODELET_FATAL("~marker_ids length should be equal to ~markers_x * ~markers_y");
 				ros::shutdown();

aruco_pose/src/genmap.py

@@ -13,7 +13,7 @@
 Generate map file for aruco_map nodelet.
 
 Usage:
-  genmap.py <length> <x> <y> <dist_x> <dist_y> [<first>] [<x0>] [<y0>] [--top-left | --bottom-left]
+  genmap.py <length> <x> <y> <dist_x> <dist_y> [<first>] [--top-left | --bottom-left]
   genmap.py (-h | --help)
 
 Options:
@@ -23,8 +23,6 @@ Options:
   <dist_x>       Distance between markers along X axis
   <dist_y>       Distance between markers along Y axis
   <first>        First marker ID [default: 0]
-  <x0>           X coordinate for the first marker [default: 0]
-  <y0>           Y coordinate for the first marker [default: 0]
   --top-left     First marker is on top-left (default)
   --bottom-left  First marker is on bottom-left
 
@@ -41,22 +39,20 @@ arguments = docopt(__doc__)
 
 length = float(arguments['<length>'])
 first = int(arguments['<first>'] if arguments['<first>'] is not None else 0)
-x0 = float(arguments['<x0>'] if arguments['<x0>'] is not None else 0)
-y0 = float(arguments['<y0>'] if arguments['<y0>'] is not None else 0)
 markers_x = int(arguments['<x>'])
 markers_y = int(arguments['<y>'])
 dist_x = float(arguments['<dist_x>'])
 dist_y = float(arguments['<dist_y>'])
 bottom_left = arguments['--bottom-left']
 
-max_y = y0 + (markers_y - 1) * dist_y
+max_y = (markers_y - 1) * dist_y
 
 print('# id\tlength\tx\ty\tz\trot_z\trot_y\trot_x')
 for y in range(markers_y):
     for x in range(markers_x):
-        pos_x = x0 + x * dist_x
-        pos_y = y0 + y * dist_y
+        pos_x = x * dist_x
+        pos_y = y * dist_y
         if not bottom_left:
             pos_y = max_y - pos_y
-        print('{}\t{}\t{}\t{}\t{}\t{}\t{}\t{}'.format(first, length, pos_x, pos_y, 0, 0, 0, 0))
+        print('{}\t{}\t{}\t{}\t{}\t{}\t{}\t{}\t'.format(first, length, pos_x, pos_y, 0, 0, 0, 0))
         first += 1

aruco_pose/src/utils.h

@@ -35,7 +35,9 @@ static void parseCameraInfo(const sensor_msgs::CameraInfoConstPtr& cinfo, cv::Ma
 	for (unsigned int i = 0; i < 3; ++i)
 		for (unsigned int j = 0; j < 3; ++j)
 			matrix.at<double>(i, j) = cinfo->K[3 * i + j];
-	dist = cv::Mat(cinfo->D, true);
+
+	for (unsigned int k = 0; k < cinfo->D.size(); k++)
+		dist.at<double>(k) = cinfo->D[k];
 }
 
 inline void rotatePoint(cv::Point3f& p, cv::Point3f origin, float angle)

aruco_pose/test/crash_opencv.py

@@ -1,18 +0,0 @@
-import rospy
-import pytest
-
-from visualization_msgs.msg import MarkerArray as VisMarkerArray
-
-
-@pytest.fixture
-def node():
-    return rospy.init_node('aruco_pose_opencv_crash', anonymous=True)
-
-def test_opencv_crashes_img01(node):
-    rospy.wait_for_message('aruco_detect_01/visualization', VisMarkerArray, timeout=5)
-
-def test_opencv_crashes_img02(node):
-    rospy.wait_for_message('aruco_detect_02/visualization', VisMarkerArray, timeout=5)
-
-def test_opencv_crashes_img03(node):
-    rospy.wait_for_message('aruco_detect_03/visualization', VisMarkerArray, timeout=5)

aruco_pose/test/crash_opencv.test

@@ -1,51 +0,0 @@
-<launch>
-    <arg name="corner_method" default="2"/>
-
-    <node pkg="image_publisher" type="image_publisher" name="imgpub_01" args="$(find aruco_pose)/test/crash_image_01.png">
-        <param name="frame_id" value="main_camera_optical"/>
-        <param name="publish_rate" value="10"/>
-        <param name="camera_info_url" value="file://$(find aruco_pose)/test/camera_info.yaml" />
-    </node>
-
-    <node pkg="image_publisher" type="image_publisher" name="imgpub_02" args="$(find aruco_pose)/test/crash_image_02.png">
-        <param name="frame_id" value="main_camera_optical"/>
-        <param name="publish_rate" value="10"/>
-        <param name="camera_info_url" value="file://$(find aruco_pose)/test/camera_info.yaml" />
-    </node>
-
-    <node pkg="image_publisher" type="image_publisher" name="imgpub_03" args="$(find aruco_pose)/test/crash_image_03.png">
-        <param name="frame_id" value="main_camera_optical"/>
-        <param name="publish_rate" value="10"/>
-        <param name="camera_info_url" value="file://$(find aruco_pose)/test/camera_info.yaml" />
-    </node>
-
-    <node pkg="nodelet" type="nodelet" name="nodelet_manager_01" args="manager"/>
-
-    <node pkg="nodelet" clear_params="true" type="nodelet" name="aruco_detect_01" args="load aruco_pose/aruco_detect nodelet_manager_01">
-        <remap from="image_raw" to="imgpub_01/image_raw"/>
-        <remap from="camera_info" to="imgpub_01/camera_info"/>
-        <param name="length" value="0.33"/>
-        <param name="cornerRefinementMethod" value="$(arg corner_method)"/>
-    </node>
-
-    <node pkg="nodelet" type="nodelet" name="nodelet_manager_02" args="manager"/>
-
-    <node pkg="nodelet" clear_params="true" type="nodelet" name="aruco_detect_02" args="load aruco_pose/aruco_detect nodelet_manager_02">
-        <remap from="image_raw" to="imgpub_02/image_raw"/>
-        <remap from="camera_info" to="imgpub_02/camera_info"/>
-        <param name="length" value="0.33"/>
-        <param name="cornerRefinementMethod" value="$(arg corner_method)"/>
-    </node>
-
-    <node pkg="nodelet" type="nodelet" name="nodelet_manager_03" args="manager"/>
-
-    <node pkg="nodelet" clear_params="true" type="nodelet" name="aruco_detect_03" args="load aruco_pose/aruco_detect nodelet_manager_03">
-        <remap from="image_raw" to="imgpub_03/image_raw"/>
-        <remap from="camera_info" to="imgpub_03/camera_info"/>
-        <param name="length" value="0.33"/>
-        <param name="cornerRefinementMethod" value="$(arg corner_method)"/>
-    </node>
-
-    <param name="test_module" value="$(find aruco_pose)/test/crash_opencv.py"/>
-    <test test-name="crash_opencv" pkg="ros_pytest" type="ros_pytest_runner"/>
-</launch>

aruco_pose/vendor/aruco/src/aruco.cpp

@@ -924,8 +924,6 @@ static void _refineCandidateLines(std::vector<Point>& nContours, std::vector<Poi
 	// calculate the line :: who passes through the grouped points
 	Point3f lines[4];
 	for(int i=0; i<4; i++){
-		// Don't try to "interpolate" single points
-		if (cntPts[i].size() < 2) return;
 		lines[i]=_interpolate2Dline(cntPts[i]);
 	}
 

builder/assets/avahi-services/sftp-ssh.service

@@ -1,34 +0,0 @@
-<?xml version="1.0" standalone='no'?><!--*-nxml-*-->
-<!DOCTYPE service-group SYSTEM "avahi-service.dtd">
-
-<!--
-  This file is part of avahi.
-
-  avahi is free software; you can redistribute it and/or modify it
-  under the terms of the GNU Lesser General Public License as
-  published by the Free Software Foundation; either version 2 of the
-  License, or (at your option) any later version.
-
-  avahi is distributed in the hope that it will be useful, but
-  WITHOUT ANY WARRANTY; without even the implied warranty of
-  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
-  General Public License for more details.
-
-  You should have received a copy of the GNU Lesser General Public
-  License along with avahi; if not, write to the Free Software
-  Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA
-  02111-1307 USA.
--->
-
-<!-- See avahi.service(5) for more information about this configuration file -->
-
-<service-group>
-
-  <name replace-wildcards="yes">%h</name>
-
-  <service>
-    <type>_sftp-ssh._tcp</type>
-    <port>22</port>
-  </service>
-
-</service-group>

builder/assets/monkey

@@ -20,7 +20,7 @@
     # Example:
     #      DocumentRoot /home/krypton/htdocs
 
-    DocumentRoot /home/pi/.ros/www
+    DocumentRoot /home/pi/catkin_ws/src/clover/clover/www
 
     # Redirect:
     # ---------

builder/image-build.sh

@@ -108,8 +108,6 @@ ${BUILDER_DIR}/image-chroot.sh ${IMAGE_PATH} exec ${SCRIPTS_DIR}'/image-software
 ${BUILDER_DIR}/image-chroot.sh ${IMAGE_PATH} copy ${SCRIPTS_DIR}'/assets/examples' '/home/pi/'
 # network setup
 ${BUILDER_DIR}/image-chroot.sh ${IMAGE_PATH} exec ${SCRIPTS_DIR}'/image-network.sh'
-# avahi setup
-${BUILDER_DIR}/image-chroot.sh ${IMAGE_PATH} copy ${SCRIPTS_DIR}'/assets/avahi-services/sftp-ssh.service' '/etc/avahi/services'
 
 # If RPi then use a one thread to build a ROS package on RPi, else use all
 [[ $(arch) == 'armv7l' ]] && NUMBER_THREADS=1 || NUMBER_THREADS=$(nproc --all)

builder/image-ros.sh

@@ -143,6 +143,7 @@ echo_stamp "Setup ROS environment"
 cat << EOF >> /home/pi/.bashrc
 LANG='C.UTF-8'
 LC_ALL='C.UTF-8'
+ROS_DISTRO='melodic'
 export ROS_HOSTNAME=\`hostname\`.local
 source /opt/ros/melodic/setup.bash
 source /home/pi/catkin_ws/devel/setup.bash

builder/image-software.sh

@@ -57,10 +57,6 @@ my_travis_retry() {
   return $result
 }
 
-echo_stamp "Increase apt retries"
-
-echo "APT::Acquire::Retries \"3\";" > /etc/apt/apt.conf.d/80-retries
-
 echo_stamp "Install apt keys & repos"
 
 # TODO: This STDOUT consist 'OK'

builder/standalone-install.sh

@@ -1,7 +1,7 @@
 #!/bin/bash
 
 # Perform a "standalone install" in a Docker container
-set -e
+
 # Step 1: Install pip
 apt update
 apt install -y curl

clover/launch/aruco.launch

@@ -1,10 +1,7 @@
 <launch>
-    <arg name="aruco_detect" default="false"/> <!-- markers recognition enabled -->
-    <arg name="aruco_map" default="false"/> <!-- markers map recognition enabled -->
-    <arg name="aruco_vpe" default="false"/> <!-- markers map positioning enabled -->
-    <arg name="placement" default="floor"/> <!-- markers placement: floor, ceiling, unknown -->
-    <arg name="length" default="0.33"/> <!-- not-in-map markers length, m -->
-    <arg name="map" default="map.txt"/> <!-- markers map file name -->
+    <arg name="aruco_detect" default="true"/>
+    <arg name="aruco_map" default="false"/>
+    <arg name="aruco_vpe" default="false"/>
 
     <!-- For additional help go to https://clover.coex.tech/aruco -->
 
@@ -13,14 +10,11 @@
         <remap from="image_raw" to="main_camera/image_raw"/>
         <remap from="camera_info" to="main_camera/camera_info"/>
         <remap from="map_markers" to="aruco_map/markers" if="$(arg aruco_map)"/>
+        <param name="cornerRefinementMethod" value="2"/>
         <param name="estimate_poses" value="true"/>
         <param name="send_tf" value="true"/>
-        <param name="known_tilt" value="map" if="$(eval placement == 'floor')"/>
-        <param name="known_tilt" value="map_flipped" if="$(eval position == 'ceiling')"/>
-        <param name="length" value="$(arg length)"/>
-        <!-- aruco detector parameters -->
-        <param name="cornerRefinementMethod" value="2"/> <!-- contour refinement -->
-        <param name="minMarkerPerimeterRate" value="0.075"/> <!-- 0.075 for 320x240, 0.0375 for 640x480 -->
+        <param name="known_tilt" value="map"/>
+        <param name="length" value="0.33"/>
     </node>
 
     <!-- aruco_map: estimate aruco map pose -->
@@ -28,9 +22,8 @@
         <remap from="image_raw" to="main_camera/image_raw"/>
         <remap from="camera_info" to="main_camera/camera_info"/>
         <remap from="markers" to="aruco_detect/markers"/>
-        <param name="map" value="$(find aruco_pose)/map/$(arg map)"/>
-        <param name="known_tilt" value="map" if="$(eval placement == 'floor')"/>
-        <param name="known_tilt" value="map_flipped" if="$(eval placement == 'ceiling')"/>
+        <param name="map" value="$(find aruco_pose)/map/map.txt"/>
+        <param name="known_tilt" value="map"/>
         <param name="image_axis" value="true"/>
         <param name="frame_id" value="aruco_map_detected" if="$(arg aruco_vpe)"/>
         <param name="frame_id" value="aruco_map" unless="$(arg aruco_vpe)"/>

clover/launch/clover.launch

@@ -1,15 +1,16 @@
 <launch>
-    <arg name="fcu_conn" default="usb"/> <!-- FCU connection type: usb, uart, tcp, udp, sitl -->
-    <arg name="fcu_ip" default="127.0.0.1"/> <!-- FCU IP adddress (if using TCP/UDP) -->
-    <arg name="fcu_sys_id" default="1"/> <!-- MAVLink system ID, noeditor -->
-    <arg name="gcs_bridge" default="tcp"/> <!-- GCS bridge type: tcp, udp, udp-b, udp-pb -->
-    <arg name="web_video_server" default="true"/> <!-- web video server enabled -->
-    <arg name="rosbridge" default="true"/> <!-- rosbridge_suite enabled, noeditor -->
-    <arg name="main_camera" default="true"/> <!-- main camera enabled -->
-    <arg name="optical_flow" default="true"/> <!-- optical flow enabled -->
-    <arg name="rangefinder_vl53l1x" default="true"/> <!-- VL53l1X rangefinder enabled -->
-    <arg name="led" default="true"/> <!-- LED strip driver enabled -->
-    <arg name="rc" default="true"/> <!-- support for mobile RC enabled -->
+    <arg name="fcu_conn" default="usb"/>
+    <arg name="fcu_ip" default="127.0.0.1"/>
+    <arg name="fcu_sys_id" default="1"/>
+    <arg name="gcs_bridge" default="tcp"/>
+    <arg name="web_video_server" default="true"/>
+    <arg name="rosbridge" default="true"/>
+    <arg name="main_camera" default="true"/>
+    <arg name="optical_flow" default="true"/>
+    <arg name="aruco" default="false"/>
+    <arg name="rangefinder_vl53l1x" default="true"/>
+    <arg name="led" default="true"/>
+    <arg name="rc" default="true"/>
     <arg name="shell" default="true"/>
 
     <!-- log formatting -->
@@ -30,7 +31,7 @@
     </node>
 
     <!-- aruco markers -->
-    <include file="$(find clover)/launch/aruco.launch"/>
+    <include file="$(find clover)/launch/aruco.launch" if="$(arg aruco)"/>
 
     <!-- optical flow -->
     <node pkg="nodelet" type="nodelet" name="optical_flow" args="load clover/optical_flow nodelet_manager" if="$(arg optical_flow)" clear_params="true" output="screen">
@@ -81,16 +82,4 @@
 
     <!-- Shell access through ROS service -->
     <node name="shell" pkg="clover" type="shell" output="screen" if="$(arg shell)"/>
-
-    <!-- Update static directory -->
-    <node pkg="roswww_static" name="roswww_static" type="main.py" clear_params="true">
-        <param name="default_package" value="clover"/>
-    </node>
-
-    <!-- roslaunch editor parameters -->
-    <group ns="roslaunch_editor">
-        <param name="items" type="yaml" value="[clover/clover.launch, clover/main_camera.launch, clover/aruco.launch, clover/led.launch]"/>
-        <param name="apply_command" value="sudo systemctl restart clover"/>
-        <param name="hide_uncommented" value="true"/>
-    </group>
 </launch>

clover/launch/led.launch

@@ -1,7 +1,7 @@
 <launch>
-    <arg name="ws281x" default="true"/> <!-- LED strip driver enabled -->
-    <arg name="led_effect" default="true"/> <!-- LED effect API enabled -->
-    <arg name="led_notify" default="all"/> <!-- LED strip notifications: all, battery, none -->
+    <arg name="ws281x" default="true"/>
+    <arg name="led_effect" default="true"/>
+    <arg name="led_notify" default="true"/>
 
     <!-- For additional help go to https://clover.coex.tech/led -->
 
@@ -22,7 +22,7 @@
         <param name="fade_period" value="0.5"/>
         <param name="rainbow_period" value="5"/>
         <!-- events effects table -->
-        <rosparam param="notify" if="$(eval led_notify == 'all')">
+        <rosparam param="notify" if="$(arg led_notify)">
             startup: { r: 255, g: 255, b: 255 }
             connected: { effect: rainbow }
             disconnected: { effect: blink, r: 255, g: 50, b: 50 }
@@ -34,8 +34,5 @@
             low_battery: { threshold: 3.7, effect: blink_fast, r: 255, g: 0, b: 0 }
             error: { effect: flash, r: 255, g: 0, b: 0 }
         </rosparam>
-        <rosparam param="notify" if="$(eval led_notify == 'battery')">
-            low_battery: { threshold: 3.7, effect: blink_fast, r: 255, g: 0, b: 0 }
-        </rosparam>
     </node>
 </launch>

clover/src/optical_flow.cpp

@@ -34,7 +34,9 @@ class OpticalFlow : public nodelet::Nodelet
 {
 public:
 	OpticalFlow():
-		camera_matrix_(3, 3, CV_64F)
+		camera_matrix_(3, 3, CV_64F),
+		dist_coeffs_(8, 1, CV_64F),
+		tf_listener_(tf_buffer_)
 	{}
 
 private:
@@ -50,8 +52,8 @@ private:
 	Mat hann_;
 	Mat prev_, curr_;
 	Mat camera_matrix_, dist_coeffs_;
-	std::unique_ptr<tf2_ros::Buffer> tf_buffer_;
-	std::unique_ptr<tf2_ros::TransformListener> tf_listener_;
+	tf2_ros::Buffer tf_buffer_;
+	tf2_ros::TransformListener tf_listener_;
 	bool calc_flow_gyro_;
 
 	void onInit()
@@ -61,14 +63,11 @@ private:
 		image_transport::ImageTransport it(nh);
 		image_transport::ImageTransport it_priv(nh_priv);
 
-		tf_buffer_.reset(new tf2_ros::Buffer());
-		tf_listener_.reset(new tf2_ros::TransformListener(*tf_buffer_, nh));
-
-		local_frame_id_ = nh.param<std::string>("mavros/local_position/tf/frame_id", "map");
-		fcu_frame_id_ = nh.param<std::string>("mavros/local_position/tf/child_frame_id", "base_link");
-		roi_px_ = nh_priv.param("roi", 128);
-		roi_rad_ = nh_priv.param("roi_rad", 0.0);
-		calc_flow_gyro_ = nh_priv.param("calc_flow_gyro", false);
+		nh.param<std::string>("mavros/local_position/tf/frame_id", local_frame_id_, "map");
+		nh.param<std::string>("mavros/local_position/tf/child_frame_id", fcu_frame_id_, "base_link");
+		nh_priv.param("roi", roi_px_, 128);
+		nh_priv.param("roi_rad", roi_rad_, 0.0);
+		nh_priv.param("calc_flow_gyro", calc_flow_gyro_, false);
 
 		img_sub_ = it.subscribeCamera("image_raw", 1, &OpticalFlow::flow, this);
 		img_pub_ = it_priv.advertise("debug", 1);
@@ -92,7 +91,9 @@ private:
 				camera_matrix_.at<double>(i, j) = cinfo->K[3 * i + j];
 			}
 		}
-		dist_coeffs_ = cv::Mat(cinfo->D, true);
+		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
@@ -185,7 +186,7 @@ private:
 			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
 			try {
-				tf_buffer_->transform(flow_camera, flow_fcu, fcu_frame_id_);
+				tf_buffer_.transform(flow_camera, flow_fcu, fcu_frame_id_);
 			} catch (const tf2::TransformException& e) {
 				// transform is not available yet
 				return;
@@ -199,7 +200,7 @@ private:
 				try {
 					auto flow_gyro_camera = calcFlowGyro(msg->header.frame_id, prev_stamp_, msg->header.stamp);
 					static geometry_msgs::Vector3Stamped flow_gyro_fcu;
-					tf_buffer_->transform(flow_gyro_camera, flow_gyro_fcu, fcu_frame_id_);
+					tf_buffer_.transform(flow_gyro_camera, flow_gyro_fcu, fcu_frame_id_);
 					flow_.integrated_xgyro = flow_gyro_fcu.vector.x;
 					flow_.integrated_ygyro = flow_gyro_fcu.vector.y;
 					flow_.integrated_zgyro = flow_gyro_fcu.vector.z;
@@ -246,8 +247,8 @@ private:
 	geometry_msgs::Vector3Stamped calcFlowGyro(const std::string& frame_id, const ros::Time& prev, const ros::Time& curr)
 	{
 		tf2::Quaternion prev_rot, curr_rot;
-		tf2::fromMsg(tf_buffer_->lookupTransform(frame_id, local_frame_id_, prev).transform.rotation, prev_rot);
-		tf2::fromMsg(tf_buffer_->lookupTransform(frame_id, local_frame_id_, curr, ros::Duration(0.1)).transform.rotation, curr_rot);
+		tf2::fromMsg(tf_buffer_.lookupTransform(frame_id, local_frame_id_, prev).transform.rotation, prev_rot);
+		tf2::fromMsg(tf_buffer_.lookupTransform(frame_id, local_frame_id_, curr, ros::Duration(0.1)).transform.rotation, curr_rot);
 
 		geometry_msgs::Vector3Stamped flow;
 		flow.header.frame_id = frame_id;

clover/src/selfcheck.py

@@ -701,7 +701,7 @@ def check_preflight_status():
 
 @check('Network')
 def check_network():
-    ros_hostname = os.environ.get('ROS_HOSTNAME', '').strip()
+    ros_hostname = os.environ.get('ROS_HOSTNAME').strip()
 
     if not ros_hostname:
         failure('no ROS_HOSTNAME is set')

clover/www/img/coex.svg

@@ -0,0 +1 @@
+<?xml version="1.0" encoding="UTF-8"?> <!-- Generator: Adobe Illustrator 22.0.1, SVG Export Plug-In . SVG Version: 6.00 Build 0) --> <svg xmlns="http://www.w3.org/2000/svg" xmlns:xlink="http://www.w3.org/1999/xlink" id="Слой_1" x="0px" y="0px" viewBox="0 0 1224.8 637.4" style="enable-background:new 0 0 1224.8 637.4;" xml:space="preserve"> <style type="text/css"> .st0{fill:#FFFFFF;} </style> <g> <g> <rect x="621.2" y="212.9" class="st0" width="160.1" height="18.5"></rect> <rect x="621.2" y="406.9" class="st0" width="160.1" height="18.5"></rect> <rect x="621.2" y="309.4" class="st0" width="160.1" height="18.5"></rect> </g> <g> <path class="st0" d="M496.7,406.5c-45.3,0-82.6-34.5-87.1-78.6H391c4.4,54.5,50.1,97.4,105.7,97.4c55.5,0,101.1-43,105.7-97.4 h-18.6C579.3,372,541.9,406.5,496.7,406.5z"></path> <path class="st0" d="M496.7,231.4c45,0,82.2,34.2,87,78h18.6c-4.9-54-50.4-96.5-105.6-96.5c-55.1,0-100.6,42.4-105.6,96.5h18.6 C414.4,265.6,451.6,231.4,496.7,231.4z"></path> </g> <g> <polygon class="st0" points="982.7,246 982.8,246 1011.9,212.9 987.2,212.9 918.3,291.1 930.6,305.1 "></polygon> <polygon class="st0" points="905.9,305.1 905.9,305.1 824.5,212.9 799.8,212.9 893.5,319.1 799.8,425.4 824.5,425.4 905.9,333.2 905.9,333.2 918.3,319.1 "></polygon> <polygon class="st0" points="918.2,347.1 987.2,425.4 1011.9,425.4 930.6,333.1 "></polygon> </g> <path class="st0" d="M230.3,318.7c0-48.4,39.3-87.7,87.7-87.7h53.9v-18.5H317c-58,0.7-105.2,48.3-105.2,106.2 c0,57.8,45.4,104.5,103.6,106.2h56.8v-18.5H318C269.6,406.4,230.3,367,230.3,318.7z"></path> </g> </svg> 

clover/www/index.html

@@ -1,3 +1,15 @@
+<style>
+	body { margin: 0; font-family: 'Roboto', Arial; }
+	header { height: 67px; background: black; position: relative; }
+	header .logo { display: block; width: 160px; height: 83px; background: url(img/coex.svg) 100% 100%;}
+	header .clover { position: absolute; right: 100px; top: 10px; font-size: 35px; } 
+</style>
+
+<header>
+	<a href="/" class=logo></a>
+	<div class=clover>&#127808;</div>
+</header>
+
 <h1>Clover Drone Kit Tools</h1>
 
 <ul>
@@ -12,8 +24,8 @@
 
 <script src="js/roslib.js"></script>
 <script type="text/javascript">
-	document.querySelector("#wvs").href = location.protocol + '//' + location.hostname + ':8080';
-	document.querySelector("#butterfly").href = location.protocol + '//' + location.hostname + ':57575';
+	document.querySelector("#wvs").href = location.origin + ':8080';
+	document.querySelector("#butterfly").href = location.origin + ':57575';
 
 	// Determine image version
 	var ros = new ROSLIB.Ros({ url: 'ws://' + location.hostname + ':9090' });