clover: Update ros3djs, THREE.js

This shebang will be ignored with newer dynamic_reconfigure packages, and its presence should allow builds to succeed when older dynamic_reconfigure is used.

.travis.yml

@@ -1,5 +1,4 @@
-os: linux
-dist: xenial
+sudo: required
 language: generic
 services:
   - docker
@@ -7,7 +6,7 @@ env:
   global:
     - DOCKER="sfalexrog/img-tool:qemu-update"
     - TARGET_REPO="https://github.com/${TRAVIS_REPO_SLUG}.git"
-    - IMAGE_VERSION=${TRAVIS_TAG:-${TRAVIS_COMMIT:0:7}}
+    - if [[ -z ${TRAVIS_TAG} ]]; then IMAGE_VERSION="${TRAVIS_COMMIT}}"; else IMAGE_VERSION="${TRAVIS_TAG}"; fi
     - IMAGE_NAME="$(basename -s '.git' ${TARGET_REPO})_${IMAGE_VERSION}.img"
 git:
   depth: 50
@@ -36,16 +35,15 @@ jobs:
           fi
       before_cache:
         - cp images/*.zip imgcache
-      after_success:
-        - sudo chmod -R 777 *
-        - cd images && zip ${IMAGE_NAME}.zip ${IMAGE_NAME} && stat --printf="Compressed image size:%s\n" ${IMAGE_NAME}.zip
       before_deploy:
         # Set up git user name and tag this commit
         - git config --local user.name "goldarte"
         - git config --local user.email "goldartt@gmail.com"
+        - sudo chmod -R 777 *
+        - cd images && zip ${IMAGE_NAME}.zip ${IMAGE_NAME}
       deploy:
         provider: releases
-        token: ${GITHUB_OAUTH_TOKEN}
+        api_key: ${GITHUB_OAUTH_TOKEN}
         file: ${IMAGE_NAME}.zip
         skip_cleanup: true
         on:
@@ -74,11 +72,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:
@@ -87,16 +82,15 @@ jobs:
         - ./check_unused_assets.py
         - gitbook install
         - gitbook build
-        - gitbook pdf ./ _book/clover.pdf
       deploy:
         provider: pages
-        local_dir: _book
-        skip_cleanup: true
-        token: ${GITHUB_OAUTH_TOKEN}
-        keep_history: true
-        target_branch: master
-        repo: CopterExpress/clover.coex.tech
-        fqdn: clover.coex.tech
+        local-dir: _book
+        skip-cleanup: true
+        github-token: ${GITHUB_OAUTH_TOKEN}
+        keep-history: true
+        target-branch: master
+        repo: CopterExpress/clever.coex.tech
+        fqdn: clever.coex.tech
         verbose: true
         on:
           branch: master
@@ -115,7 +109,7 @@ jobs:
         - wget https://github.com/okalachev/editorconfig-checker/releases/download/1.2.1-disable-spaces-amount/ec-linux-amd64
         - chmod +x ec-linux-amd64
       script:
-        - ./ec-linux-amd64 -spaces-after-tabs -e "roslib.js|ros3d.js|eventemitter2.js|draw.cpp|BinUtils.swift|\.idea|apps/android/app|Assets.xcassets|test_parser_pass.txt|test_node_failure.txt|aruco_pose/vendor|\.stl|\.dxf|\.dae"
+        - ./ec-linux-amd64 -spaces-after-tabs -e "roslib.js|ros3d.js|eventemitter2.js|draw.cpp|BinUtils.swift|\.idea|apps/android/app|Assets.xcassets|test_parser_pass.txt|test_node_failure.txt|aruco_pose/vendor|\.stl|\.dxf"
 stages:
   - Build
   - Annotate

README.md

@@ -1,40 +1,103 @@
-# COEX Clover Drone Kit
+# CLEVER
 
-<img src="docs/assets/clever4-front-white.png" align="right" width="400px" alt="Clover Drone">
+<img src="docs/assets/clever4-front-white.png" align="right" width="400px" alt="CLEVER drone">
 
-Clover is an educational programmable drone kit consisting of an unassembled quadcopter, open source software and documentation. The kit includes Pixracer-compatible autopilot running PX4 firmware, Raspberry Pi 4 as companion computer, a camera for computer vision navigation as well as additional sensors and peripheral devices.
+CLEVER (Russian: *"Клевер"*, meaning *"Clover"*) is an educational programmable drone kit consisting of an unassembled quadcopter, open source software and documentation. The kit includes Pixhawk/Pixracer autopilot running PX4 firmware, Raspberry Pi 3 as companion computer, a camera for computer vision navigation as well as additional sensors and peripheral devices.
 
-The main documentation is available [on Gitbook](https://clover.coex.tech/).
+Copter Express has implemented a large number of different autonomous drone projects using exactly the same platform: [automated pizza delivery](https://www.youtube.com/watch?v=hmkAoZOtF58) in Samara and Kazan, coffee delivery in Skolkovo Innovation Center, [autonomous quadcopter with charging station](https://www.youtube.com/watch?v=RjX6nUqw1mI) for site monitoring and security, winning drones on [Robocross-2016](https://www.youtube.com/watch?v=dGbDaz_VmYU) and [Robocross-2017](https://youtu.be/AQnd2CRczbQ) competitions and many others.
 
-Official website: <a href="https://coex.tech/clover">coex.tech/clover</a>.
+**The main documentation is available [on Gitbook](https://clever.coex.tech/).**
 
-## Video compilation
-
-[![Clover Drone Kit autonomy compilation](http://img.youtube.com/vi/u3omgsYC4Fk/hqdefault.jpg)](https://youtu.be/u3omgsYC4Fk)
-
-Clover drone is used on a wide range of educational events, including [Copter Hack](https://www.youtube.com/watch?v=xgXheg3TTs4), WorldSkills Drone Operation competition, [Autonomous Vehicles Track of NTI Olympics 2016–2020](https://www.youtube.com/watch?v=E1_ehvJRKxg), Quadro Hack 2019 (National University of Science and Technology MISiS), Russian Robot Olympiad (autonomous flights), and others.
+Use it to learn how to assemble, configure, pilot and program autonomous CLEVER drone.
 
 ## Raspberry Pi image
 
-Preconfigured image for Raspberry Pi with installed and configured software, ready to fly, is available [in the Releases section](https://github.com/CopterExpress/clover/releases).
+**Preconfigured image for Raspberry Pi 3 with installed and configured software, ready to fly, is available [in the Releases section](https://github.com/CopterExpress/clever/releases).**
 
-[![Build Status](https://travis-ci.org/CopterExpress/clover.svg?branch=master)](https://travis-ci.org/CopterExpress/clover)
+[![Build Status](https://travis-ci.org/CopterExpress/clever.svg?branch=master)](https://travis-ci.org/CopterExpress/clever)
 
-Image features:
+Image includes:
 
 * Raspbian Buster
-* [ROS Melodic](http://wiki.ros.org/melodic)
+* ROS Melodic
 * Configured networking
 * OpenCV
-* [`mavros`](http://wiki.ros.org/mavros)
-* Periphery drivers for ROS ([GPIO](https://clover.coex.tech/en/gpio.html), [LED strip](https://clover.coex.tech/en/leds.html), etc)
-* `aruco_pose` package for marker-assisted navigation
-* `clover` package for autonomous drone control
+* mavros
+* Periphery drivers (`pigpiod`, `rpi_ws281x`, etc)
+* CLEVER software bundle for autonomous drone control
 
-API description for autonomous flights is available [on GitBook](https://clover.coex.tech/en/simple_offboard.html).
+API description (in Russian) for autonomous flights is available [on GitBook](https://clever.coex.tech/simple_offboard.html).
 
-For manual package installation and running see [`clover` package documentation](clover/README.md).
+## Manual installation
+
+Install ROS Melodic according to the [documentation](http://wiki.ros.org/melodic/Installation), then [create a Catkin workspace](http://wiki.ros.org/catkin/Tutorials/create_a_workspace).
+
+Clone this repo to directory `~/catkin_ws/src/clever`:
+
+```bash
+cd ~/catkin_ws/src
+git clone https://github.com/CopterExpress/clever.git clever
+```
+
+All the required ROS packages (including `mavros` and `opencv`) can be installed using `rosdep`:
+
+```bash
+cd ~/catkin_ws/
+rosdep install -y --from-paths src --ignore-src
+```
+
+Build ROS packages (on memory constrained platforms you might be going to need to use `-j1` key):
+
+```bash
+cd ~/catkin_ws
+catkin_make -j1
+```
+
+To complete `mavros` install you'll need to install `geographiclib` datasets:
+
+```bash
+curl https://raw.githubusercontent.com/mavlink/mavros/master/mavros/scripts/install_geographiclib_datasets.sh | sudo bash
+```
+
+You may optionally install udev rules to provide `/dev/px4fmu` symlink to your PX4-based flight controller connected over USB. Copy `99-px4fmu.rules` to your `/lib/udev/rules.d` folder:
+
+```bash
+cd ~/catkin_ws/src/clever/clever/config
+sudo cp 99-px4fmu.rules /lib/udev/rules.d
+```
+
+Alternatively you may change the `fcu_url` property in `mavros.launch` file to point to your flight controller device.
+
+## Running
+
+Enable systemd service `roscore` (if not running):
+
+```bash
+sudo systemctl enable /home/<username>/catkin_ws/src/clever/builder/assets/roscore.service
+sudo systemctl start roscore
+```
+
+To start connection to SITL, use:
+
+```bash
+roslaunch clever sitl.launch
+```
+
+To start connection to the flight controller, use:
+
+```bash
+roslaunch clever clever.launch
+```
+
+> Note that the package is configured to connect to `/dev/px4fmu` by default (see [previous section](#manual-installation)). Install udev rules or specify path to your FCU device in `mavros.launch`.
+
+Also, you can enable and start the systemd service:
+
+```bash
+sudo systemctl enable /home/<username>/catkin_ws/src/clever/deploy/clever.service
+sudo systemctl start clever
+```
 
 ## License
 
-While the Clover platform source code is available under the MIT License, note, that the [documentation](docs/) is licensed under the Creative Commons Attribution-NonCommercial-ShareAlike 4.0 International License.
+While the Clever platform source code is available under the MIT License, note, that the [documentation](docs/) is licensed under the Creative Commons Attribution-NonCommercial-ShareAlike 4.0 International License.

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();
@@ -392,7 +394,7 @@ publish_debug:
 		int num_markers = board_->dictionary->bytesList.rows;
 		if (num_markers <= id) {
 			NODELET_ERROR("Marker id %d is not in dictionary; current dictionary contains %d markers. "
-			              "Please see https://github.com/CopterExpress/clover/blob/master/aruco_pose/README.md#parameters for details",
+			              "Please see https://github.com/CopterExpress/clever/blob/master/aruco_pose/README.md#parameters for details",
 					  id, num_markers);
 			return;
 		}

aruco_pose/src/genmap.py

@@ -1,4 +1,4 @@
-#!/usr/bin/env python
+#!/usr/bin/env python3
 
 # Copyright (C) 2018 Copter Express Technologies
 #
@@ -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/test/largemap.py

@@ -1,26 +1,19 @@
-#!/usr/bin/env python
-import sys
-import unittest
-import json
 import rospy
-import rostest
+import pytest
 
 from sensor_msgs.msg import Image
 from visualization_msgs.msg import MarkerArray as VisMarkerArray
 
+@pytest.fixture
+def node():
+    return rospy.init_node('test_aruco_largemap', anonymous=True)
 
-class TestArucoPose(unittest.TestCase):
-    def setUp(self):
-        rospy.init_node('test_aruco_largemap', anonymous=True)
+def test_large_map_image(node):
+    img = rospy.wait_for_message('aruco_map/image', Image, timeout=5)
+    assert img.width == 2000
+    assert img.height == 2000
+    assert img.encoding in ('mono8', 'rgb8')
 
-    def test_map_image(self):
-        img = rospy.wait_for_message('aruco_map/image', Image, timeout=5)
-        self.assertEqual(img.width, 2000)
-        self.assertEqual(img.height, 2000)
-        self.assertIn(img.encoding, ('mono8', 'rgb8'))
-
-    def test_map_visualization(self):
-        vis = rospy.wait_for_message('aruco_map/visualization', VisMarkerArray, timeout=5)
-
-
-rostest.rosrun('aruco_pose', 'test_aruco_largemap', TestArucoPose, sys.argv)
+def test_large_map_visualization(node):
+    vis = rospy.wait_for_message('aruco_map/visualization', VisMarkerArray, timeout=5)
+    assert len(vis.markers) == 11

aruco_pose/test/largemap.test

@@ -9,5 +9,6 @@
         <param name="map" value="$(find aruco_pose)/test/largemap.txt"/>
     </node>
 
-    <test test-name="test_aruco_pose_largemap" pkg="aruco_pose" type="largemap.py"/>
+    <param name="test_module" value="$(find aruco_pose)/test/largemap.py"/>
+    <test test-name="test_node_pass" 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]);
 	}
 

book.json

@@ -1,5 +1,5 @@
 {
-    "title": "Clover",
+    "title": "Clever",
     "description": "Конструктор квадрокоптера «Клевер»",
     "author": "Copter Express",
     "language": "en",
@@ -28,7 +28,7 @@
             "blank": true
         },
         "sitemap": {
-            "hostname": "https://clover.coex.tech"
+            "hostname": "https://clever.coex.tech"
         },
         "toolbar": {
             "buttons":
@@ -37,19 +37,19 @@
                     "label": "Edit page on github",
                     "icon": "fa fa-pencil-square-o",
                     "position" : "left",
-                    "url": "https://github.com/CopterExpress/clover/edit/master/docs/{{filepath_lang}}"
+                    "url": "https://github.com/CopterExpress/clever/edit/master/docs/{{filepath_lang}}"
                 },
                 {
                     "label": "GitHub",
                     "icon": "fa fa-github",
                     "position" : "left",
-                    "url": "https://github.com/CopterExpress/clover"
+                    "url": "https://github.com/CopterExpress/clever"
                 }
             ]
         },
         "addcssjs": {
-            "css": ["../clover.css"],
-            "js": ["../clover.js"]
+            "css": ["../clever.css"],
+            "js": ["../clever.js"]
         },
         "language-picker": {
             "languages": [["ru", "Russian"], ["en", "English"]]

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/clever/setup.py

@@ -1,4 +1,4 @@
-#!/usr/bin/env python
+#!/usr/bin/env python3
 
 from distutils.core import setup
 

builder/assets/examples/flight.py

@@ -1,4 +1,4 @@
-# Information: https://clover.coex.tech/programming
+# Information: https://clever.coex.tech/en/programming.html
 
 import rospy
 from clover import srv
@@ -15,7 +15,7 @@ set_attitude = rospy.ServiceProxy('set_attitude', srv.SetAttitude)
 set_rates = rospy.ServiceProxy('set_rates', srv.SetRates)
 land = rospy.ServiceProxy('land', Trigger)
 
-# Take off and hover 1 m above the ground
+# Takeoff and hover 1 m above the ground
 navigate(x=0, y=0, z=1, frame_id='body', auto_arm=True)
 
 # Wait for 3 seconds

builder/assets/examples/flight_marker.py

@@ -1,37 +0,0 @@
-# Information: https://clover.coex.tech/en/aruco.html
-
-import rospy
-from clover import srv
-from std_srvs.srv import Trigger
-
-rospy.init_node('flight')
-
-get_telemetry = rospy.ServiceProxy('get_telemetry', srv.GetTelemetry)
-navigate = rospy.ServiceProxy('navigate', srv.Navigate)
-navigate_global = rospy.ServiceProxy('navigate_global', srv.NavigateGlobal)
-set_position = rospy.ServiceProxy('set_position', srv.SetPosition)
-set_velocity = rospy.ServiceProxy('set_velocity', srv.SetVelocity)
-set_attitude = rospy.ServiceProxy('set_attitude', srv.SetAttitude)
-set_rates = rospy.ServiceProxy('set_rates', srv.SetRates)
-land = rospy.ServiceProxy('land', Trigger)
-
-# Take off and hover 1 m above the ground
-navigate(x=0, y=0, z=1, frame_id='body', auto_arm=True)
-
-# Wait for 3 seconds
-rospy.sleep(3)
-
-# Fly 1 meter above ArUco marker 0
-navigate(x=0, y=0, z=1, frame_id='aruco_0')
-
-# Wait for 3 seconds
-rospy.sleep(3)
-
-# Fly to x=1 y=1 z=1 relative to ArUco markers map
-navigate(x=1, y=1, z=1, frame_id='aruco_map')
-
-# Wait for 3 seconds
-rospy.sleep(3)
-
-# Perform landing
-land()

builder/assets/examples/leds.py

@@ -1,4 +1,4 @@
-# Information: https://clover.coex.tech/en/leds.html
+# Information: https://clever.coex.tech/en/leds.html
 
 import rospy
 from clover.srv import SetLEDEffect

builder/assets/examples/navigate_wait.py

@@ -1,41 +0,0 @@
-# Information: https://clover.coex.tech/en/snippets.html#block-nav
-
-import math
-import rospy
-from clover import srv
-from std_srvs.srv import Trigger
-
-rospy.init_node('flight')
-
-get_telemetry = rospy.ServiceProxy('get_telemetry', srv.GetTelemetry)
-navigate = rospy.ServiceProxy('navigate', srv.Navigate)
-navigate_global = rospy.ServiceProxy('navigate_global', srv.NavigateGlobal)
-set_position = rospy.ServiceProxy('set_position', srv.SetPosition)
-set_velocity = rospy.ServiceProxy('set_velocity', srv.SetVelocity)
-set_attitude = rospy.ServiceProxy('set_attitude', srv.SetAttitude)
-set_rates = rospy.ServiceProxy('set_rates', srv.SetRates)
-land = rospy.ServiceProxy('land', Trigger)
-
-def navigate_wait(x=0, y=0, z=0, yaw=float('nan'), yaw_rate=0, speed=0.5, \
-        frame_id='body', tolerance=0.2, auto_arm=False):
-
-    res = navigate(x=x, y=y, z=z, yaw=yaw, yaw_rate=yaw_rate, speed=speed, \
-        frame_id=frame_id, auto_arm=auto_arm)
-
-    if not res.success:
-        return res
-
-    while not rospy.is_shutdown():
-        telem = get_telemetry(frame_id='navigate_target')
-        if math.sqrt(telem.x ** 2 + telem.y ** 2 + telem.z ** 2) < tolerance:
-            return res
-        rospy.sleep(0.2)
-
-# Take off 1 meter
-navigate_wait(z=1, frame_id='body', auto_arm=True)
-
-# Fly forward 1 m
-navigate_wait(x=1, frame_id='body')
-
-# Land
-land()

builder/assets/init_rpi.sh

@@ -62,10 +62,6 @@ hostnamectl set-hostname $NEW_HOSTNAME
 sed -i 's/127\.0\.1\.1.*/127.0.1.1\t'${NEW_HOSTNAME}' '${NEW_HOSTNAME}'.local/g' /etc/hosts
 # .local (mdns) hostname added to make it accesable when wlan and ethernet interfaces are down
 
-echo_stamp "Enable ROS services"
-systemctl enable roscore
-systemctl enable clover
-
 echo_stamp "Harware setup"
 /root/hardware_setup.sh
 

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/assets/python3.yaml

@@ -0,0 +1,9 @@
+python3-wxgtk:
+  debian:
+    buster: [python3-wxgtk4.0]
+python3-serial:
+  debian:
+    buster: [python3-serial]
+python3-requests:
+  debian:
+    buster: [python3-requests]

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)
@@ -117,6 +115,7 @@ ${BUILDER_DIR}/image-chroot.sh ${IMAGE_PATH} copy ${SCRIPTS_DIR}'/assets/avahi-s
 ${BUILDER_DIR}/image-chroot.sh ${IMAGE_PATH} copy ${SCRIPTS_DIR}'/assets/clover.service' '/lib/systemd/system/'
 ${BUILDER_DIR}/image-chroot.sh ${IMAGE_PATH} copy ${SCRIPTS_DIR}'/assets/roscore.service' '/lib/systemd/system/'
 ${BUILDER_DIR}/image-chroot.sh ${IMAGE_PATH} copy ${SCRIPTS_DIR}'/assets/melodic-rosdep-clover.yaml' '/etc/ros/rosdep/'
+${BUILDER_DIR}/image-chroot.sh ${IMAGE_PATH} copy ${SCRIPTS_DIR}'/assets/python3.yaml' '/etc/ros/rosdep/'
 ${BUILDER_DIR}/image-chroot.sh ${IMAGE_PATH} copy ${SCRIPTS_DIR}'/assets/ros_python_paths' '/etc/sudoers.d/'
 ${BUILDER_DIR}/image-chroot.sh ${IMAGE_PATH} copy ${SCRIPTS_DIR}'/assets/pigpiod.service' '/lib/systemd/system/'
 ${BUILDER_DIR}/image-chroot.sh ${IMAGE_PATH} copy ${SCRIPTS_DIR}'/assets/launch.nanorc' '/usr/share/nano/'

builder/image-ros.sh

@@ -68,13 +68,27 @@ my_travis_retry() {
 # TODO: 'kinetic-rosdep-clover.yaml' should add only if we use our repo?
 echo_stamp "Init rosdep"
 my_travis_retry rosdep init
-echo "yaml file:///etc/ros/rosdep/melodic-rosdep-clover.yaml" >> /etc/ros/rosdep/sources.list.d/20-default.list
+echo "yaml file:///etc/ros/rosdep/melodic-rosdep-clover.yaml" > /etc/ros/rosdep/sources.list.d/30-clover.list
+echo "yaml file:///etc/ros/rosdep/python3.yaml" > /etc/ros/rosdep/sources.list.d/40-python3.list
 my_travis_retry rosdep update
 
 echo_stamp "Populate rosdep for ROS user"
 my_travis_retry sudo -u pi rosdep update
 
+resolve_rosdep() {
+  # TEMPLATE: resolve_rosdep <CATKIN_PATH> <ROS_DISTRO> <OS_DISTRO> <OS_VERSION>
+  CATKIN_PATH=$1
+  ROS_DISTRO='melodic'
+  OS_DISTRO='debian'
+  OS_VERSION='buster'
+
+  echo_stamp "Installing dependencies apps with rosdep in ${CATKIN_PATH}"
+  cd ${CATKIN_PATH}
+  my_travis_retry rosdep install -y --from-paths src --ignore-src --rosdistro ${ROS_DISTRO} --os=${OS_DISTRO}:${OS_VERSION}
+}
+
 export ROS_IP='127.0.0.1' # needed for running tests
+export ROS_PYTHON_VERSION=3
 
 echo_stamp "Reconfiguring Clover repository for simplier unshallowing"
 cd /home/pi/catkin_ws/src/clover
@@ -82,14 +96,15 @@ git config remote.origin.fetch "+refs/heads/*:refs/remotes/origin/*"
 
 echo_stamp "Build and install Clover"
 cd /home/pi/catkin_ws
-# Don't try to install gazebo_ros
-my_travis_retry rosdep install -y --from-paths src --ignore-src --rosdistro melodic --os=debian:buster \
-  --skip-keys=gazebo_ros --skip-keys=gazebo_plugins
-my_travis_retry pip install wheel
-my_travis_retry pip install -r /home/pi/catkin_ws/src/clover/clover/requirements.txt
+resolve_rosdep $(pwd)
+my_travis_retry pip3 install wheel
+my_travis_retry pip3 install -r /home/pi/catkin_ws/src/clover/clover/requirements.txt
 source /opt/ros/melodic/setup.bash
-# Don't build simulation plugins for actual drone
-catkin_make -j2 -DCMAKE_BUILD_TYPE=Release -DCATKIN_BLACKLIST_PACKAGES=clover_gazebo_plugins
+catkin_make -j2 -DCMAKE_BUILD_TYPE=Release
+
+echo_stamp "Enable ROS services"
+systemctl enable roscore
+systemctl enable clover
 
 echo_stamp "Install clever package (for backwards compatibility)"
 cd /home/pi/catkin_ws/src/clover/builder/assets/clever
@@ -104,7 +119,7 @@ gitbook build
 touch node_modules/CATKIN_IGNORE docs/CATKIN_IGNORE _book/CATKIN_IGNORE clover/www/CATKIN_IGNORE apps/CATKIN_IGNORE # ignore documentation files by catkin
 
 echo_stamp "Installing additional ROS packages"
-my_travis_retry apt-get install -y --no-install-recommends \
+apt-get install -y --no-install-recommends \
     ros-melodic-dynamic-reconfigure \
     ros-melodic-compressed-image-transport \
     ros-melodic-rosbridge-suite \
@@ -120,7 +135,7 @@ echo_stamp "Install GeographicLib datasets (needed for mavros)" \
 
 # FIXME: Buster comes with tornado==5.1.1 but we need tornado==4.2.1 for rosbridge_suite
 # (note that Python 3 will still have a more recent version)
-pip install tornado==4.2.1
+pip3 install tornado==4.2.1
 
 echo_stamp "Running tests"
 cd /home/pi/catkin_ws
@@ -134,6 +149,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'
@@ -71,7 +67,7 @@ apt-get update \
 
 echo "deb http://packages.ros.org/ros/ubuntu buster main" > /etc/apt/sources.list.d/ros-latest.list
 echo "deb http://deb.coex.tech/opencv3 buster main" > /etc/apt/sources.list.d/opencv3.list
-echo "deb http://deb.coex.tech/rpi-ros-melodic buster main" > /etc/apt/sources.list.d/rpi-ros-melodic.list
+echo "deb http://deb.coex.tech/melodic-py3 buster main" > /etc/apt/sources.list.d/rpi-ros-melodic.list
 echo "deb http://deb.coex.tech/clover buster main" > /etc/apt/sources.list.d/clover.list
 
 echo_stamp "Update apt cache"
@@ -80,9 +76,8 @@ echo_stamp "Update apt cache"
 apt-get update
 # && apt upgrade -y
 
-# Let's retry fetching those packages several times, just in case
 echo_stamp "Software installing"
-my_travis_retry apt-get install --no-install-recommends -y \
+apt-get install --no-install-recommends -y \
 unzip \
 zip \
 ipython \
@@ -100,23 +95,25 @@ libjpeg8 \
 tcpdump \
 ltrace \
 libpoco-dev \
-libzbar0 \
-python-rosdep \
-python-rosinstall-generator \
-python-wstool \
-python-rosinstall \
+python3-rosdep \
+python3-rosinstall-generator \
+python3-wstool \
+python3-rosinstall \
 build-essential \
 libffi-dev \
 monkey \
 pigpio python-pigpio python3-pigpio \
 i2c-tools \
-espeak espeak-data python-espeak \
+espeak espeak-data python3-espeak \
 ntpdate \
 python-dev \
 python3-dev \
-python-systemd \
+python3-venv \
+python3-systemd \
 mjpg-streamer \
-python3-opencv
+python3-opencv \
+&& echo_stamp "Everything was installed!" "SUCCESS" \
+|| (echo_stamp "Some packages wasn't installed!" "ERROR"; exit 1)
 
 # Deny byobu to check available updates
 sed -i "s/updates_available//" /usr/share/byobu/status/status
@@ -136,13 +133,14 @@ pip3 --version
 
 echo_stamp "Install and enable Butterfly (web terminal)"
 echo_stamp "Workaround for tornado >= 6.0 breaking butterfly"
-my_travis_retry pip3 install tornado==5.1.1
+my_travis_retry pip3 install tornado==4.2.1
 my_travis_retry pip3 install butterfly
 my_travis_retry pip3 install butterfly[systemd]
 systemctl enable butterfly.socket
 
 echo_stamp "Install ws281x library"
-my_travis_retry pip install --prefer-binary rpi_ws281x
+my_travis_retry pip2 install --prefer-binary rpi_ws281x
+my_travis_retry pip3 install --prefer-binary rpi_ws281x
 
 echo_stamp "Setup Monkey"
 mv /etc/monkey/sites/default /etc/monkey/sites/default.orig
@@ -158,13 +156,9 @@ rm -rf node-v10.15.0-linux-armv6l/
 rm node-v10.15.0-linux-armv6l.tar.gz
 
 echo_stamp "Installing ptvsd"
-my_travis_retry pip install ptvsd
+my_travis_retry pip2 install ptvsd
 my_travis_retry pip3 install ptvsd
 
-echo_stamp "Installing pyzbar"
-my_travis_retry pip install pyzbar
-my_travis_retry pip3 install pyzbar
-
 echo_stamp "Add .vimrc"
 cat << EOF > /home/pi/.vimrc
 set mouse-=a

builder/image-validate.sh

@@ -18,13 +18,15 @@ echo "Run image tests"
 
 export ROS_DISTRO='melodic'
 export ROS_IP='127.0.0.1'
+export ROS_PYTHON_VERSION=3
 source /opt/ros/melodic/setup.bash
 source /home/pi/catkin_ws/devel/setup.bash
 
 cd /home/pi/catkin_ws/src/clover/builder/test/
 ./tests.sh
 ./tests.py
-./tests_py3.py
+# Disable Python 2 tests for image - we're dropping support, right?
+# ./tests_py2.py
 [[ $(./tests_clever.py) == "Warning: clever package is renamed to clover" ]]  # test backwards compatibility
 
 echo "Move /etc/ld.so.preload back to its original position"

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

builder/test/tests.py

@@ -1,4 +1,4 @@
-#!/usr/bin/env python
+#!/usr/bin/env python3
 
 # validate all required modules installed
 
@@ -17,6 +17,8 @@ from std_srvs.srv import Trigger
 from clover.srv import GetTelemetry, Navigate, NavigateGlobal, SetPosition, SetVelocity, \
     SetAttitude, SetRates, SetLEDEffect
 
+get_telemetry = rospy.ServiceProxy('get_telemetry', GetTelemetry)
+
 import tf2_ros
 import tf2_geometry_msgs
 
@@ -26,6 +28,5 @@ from pymavlink import mavutil
 import rpi_ws281x
 import pigpio
 from espeak import espeak
-from pyzbar import pyzbar
 
-print cv2.getBuildInformation()
+print(cv2.getBuildInformation())

builder/test/tests_clever.py

@@ -1,4 +1,4 @@
-#!/usr/bin/env python
+#!/usr/bin/env python3
 
 # test backwards compatibility
 

builder/test/tests_py2.py

@@ -0,0 +1,7 @@
+#!/usr/bin/env python
+
+# Make sure our Python 2 software is installed
+
+import cv2
+
+print(cv2.getBuildInformation())

builder/test/tests_py3.py

@@ -1,8 +0,0 @@
-#!/usr/bin/env python3
-
-# Make sure our Python 3 software is installed
-
-import cv2
-from pyzbar import pyzbar
-
-print(cv2.getBuildInformation())

clover/CMakeLists.txt

@@ -30,12 +30,6 @@ list(APPEND CMAKE_MODULE_PATH "${CMAKE_CURRENT_LIST_DIR}/cmake")
 
 find_package(GeographicLib REQUIRED)
 
-find_package(OpenCV 3 REQUIRED
-  COMPONENTS
-    calib3d
-    imgproc
-)
-
 ## System dependencies are found with CMake's conventions
 # find_package(Boost REQUIRED COMPONENTS system)
 
@@ -166,14 +160,13 @@ add_library(${PROJECT_NAME}
 ## The recommended prefix ensures that target names across packages don't collide
 add_executable(simple_offboard src/simple_offboard.cpp)
 
-# PX4 already has rc and led targets, so we prefix ours with clover_
-add_executable(clover_rc src/rc.cpp)
+add_executable(rc src/rc.cpp)
 
 add_executable(camera_markers src/camera_markers.cpp)
 
 add_executable(vpe_publisher src/vpe_publisher.cpp)
 
-add_executable(clover_led src/led.cpp)
+add_executable(led src/led.cpp)
 
 add_executable(shell src/shell.cpp)
 
@@ -182,24 +175,19 @@ target_link_libraries(simple_offboard
   ${GeographicLib_LIBRARIES}
 )
 
-# Don't change actual binary names
-set_target_properties(clover_rc PROPERTIES OUTPUT_NAME rc)
-
-set_target_properties(clover_led PROPERTIES OUTPUT_NAME led)
-
-target_link_libraries(clover_rc ${catkin_LIBRARIES})
+target_link_libraries(rc ${catkin_LIBRARIES})
 
 target_link_libraries(camera_markers ${catkin_LIBRARIES})
 
 target_link_libraries(vpe_publisher ${catkin_LIBRARIES})
 
-target_link_libraries(clover_led ${catkin_LIBRARIES})
+target_link_libraries(led ${catkin_LIBRARIES})
 
 target_link_libraries(shell ${catkin_LIBRARIES})
 
 add_dependencies(simple_offboard ${PROJECT_NAME}_generate_messages_cpp)
 
-add_dependencies(clover_led ${PROJECT_NAME}_generate_messages_cpp)
+add_dependencies(led ${PROJECT_NAME}_generate_messages_cpp)
 
 add_dependencies(shell ${PROJECT_NAME}_generate_messages_cpp)
 
@@ -216,7 +204,6 @@ add_dependencies(shell ${PROJECT_NAME}_generate_messages_cpp)
 ## Specify libraries to link a library or executable target against
 target_link_libraries(${PROJECT_NAME}
   ${catkin_LIBRARIES}
-  ${OpenCV_LIBRARIES}
 )
 
 #############

clover/README.md

@@ -1,59 +0,0 @@
-# `clover` ROS package
-
-A bundle for autonomous navigation and drone control.
-
-## Manual installation
-
-Install ROS Melodic according to the [documentation](http://wiki.ros.org/melodic/Installation), then [create a Catkin workspace](http://wiki.ros.org/catkin/Tutorials/create_a_workspace).
-
-Clone this repo to directory `~/catkin_ws/src/clover`:
-
-```bash
-cd ~/catkin_ws/src
-git clone https://github.com/CopterExpress/clover.git clover
-```
-
-All the required ROS packages (including `mavros` and `opencv`) can be installed using `rosdep`:
-
-```bash
-cd ~/catkin_ws/
-rosdep install -y --from-paths src --ignore-src
-```
-
-Build ROS packages (on memory constrained platforms you might be going to need to use `-j1` key):
-
-```bash
-cd ~/catkin_ws
-catkin_make -j1
-```
-
-To complete `mavros` install you'll need to install `geographiclib` datasets:
-
-```bash
-curl https://raw.githubusercontent.com/mavlink/mavros/master/mavros/scripts/install_geographiclib_datasets.sh | sudo bash
-```
-
-You may optionally install udev rules to provide `/dev/px4fmu` symlink to your PX4-based flight controller connected over USB. Copy `99-px4fmu.rules` to your `/lib/udev/rules.d` folder:
-
-```bash
-cd ~/catkin_ws/src/clover/clover/config
-sudo cp 99-px4fmu.rules /lib/udev/rules.d
-```
-
-Alternatively you may change the `fcu_url` property in `mavros.launch` file to point to your flight controller device.
-
-## Running
-
-To start connection to SITL, use:
-
-```bash
-roslaunch clover sitl.launch
-```
-
-To start connection to the flight controller, use:
-
-```bash
-roslaunch clover clover.launch
-```
-
-> Note that the package is configured to connect to `/dev/px4fmu` by default (see [previous section](#manual-installation)). Install udev rules or specify path to your FCU device in `mavros.launch`.

clover/camera_info/fisheye_cam_320.yaml

@@ -0,0 +1,45 @@
+image_width: 320
+image_height: 240
+distortion_model: plumb_bob
+camera_name: raspicam
+camera_matrix:
+  rows: 3
+  cols: 3
+  data:
+  - 166.23942373073172
+  - 0.
+  - 162.19011246829268
+  - 0.
+  - 166.5880923974026
+  - 109.82227735714285
+  - 0.
+  - 0.
+  - 1.
+distortion_coefficients:
+  rows: 1
+  cols: 8
+  data:  [  2.15356885e-01,  -1.17472846e-01,  -3.06197672e-04,
+   -1.09444025e-04,  -4.53657258e-03,   5.73090623e-01,
+   -1.27574577e-01,  -2.86125589e-02,   0.00000000e+00,
+    0.00000000e+00,   0.00000000e+00,   0.00000000e+00,
+    0.00000000e+00,   0.00000000e+00]
+rectification_matrix:
+  rows: 3
+  cols: 3
+  data: [1, 0, 0, 0, 1, 0, 0, 0, 1]
+projection_matrix:
+  rows: 3
+  cols: 4
+  data:
+  - 166.23942373073172
+  - 0.
+  - 162.19011246829268
+  - 0.
+  - 0.
+  - 166.5880923974026
+  - 109.82227735714285
+  - 0.
+  - 0.
+  - 0.
+  - 1.
+  - 0.

clover/camera_info/fisheye_cam_640.yaml

@@ -1,17 +1,17 @@
 image_width: 640
 image_height: 480
 distortion_model: plumb_bob
-camera_name: main_camera_optical
+camera_name: raspicam
 camera_matrix:
   rows: 3
   cols: 3
   data:
   - 332.47884746146343
   - 0.
-  - 320.0
+  - 324.38022493658536
   - 0.
   - 333.1761847948052
-  - 240.0
+  - 219.6445547142857
   - 0.
   - 0.
   - 1.

clover/launch/aruco.launch

@@ -3,20 +3,18 @@
     <arg name="aruco_map" default="false"/>
     <arg name="aruco_vpe" default="false"/>
 
-    <!-- For additional help go to https://clover.coex.tech/aruco -->
+    <!-- For additional help go to https://clever.coex.tech/aruco -->
 
     <!-- aruco_detect: detect aruco markers, estimate poses -->
     <node name="aruco_detect" pkg="nodelet" if="$(arg aruco_detect)" type="nodelet" args="load aruco_pose/aruco_detect nodelet_manager" output="screen" clear_params="true">
         <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"/>
         <param name="length" value="0.33"/>
-        <!-- aruco detector parameters -->
-        <param name="cornerRefinementMethod" value="2"/> <!-- contour refinement -->
-        <param name="minMarkerPerimeterRate" value="0.075"/> <!-- 0.075 for 320x240, 0.0375 for 640x480 -->
     </node>
 
     <!-- aruco_map: estimate aruco map pose -->

clover/launch/clover.launch

@@ -1,7 +1,6 @@
 <launch>
     <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"/>
@@ -13,8 +12,6 @@
     <arg name="rc" default="true"/>
     <arg name="shell" default="true"/>
 
-    <arg name="simulator" default="false"/> <!-- flag that we are operating on a simulated drone -->
-
     <!-- log formatting -->
     <env name="ROSCONSOLE_FORMAT" value="[${severity}] [${time}]: ${logger}: ${message}"/>
 
@@ -22,7 +19,6 @@
     <include file="$(find clover)/launch/mavros.launch">
         <arg name="fcu_conn" value="$(arg fcu_conn)"/>
         <arg name="fcu_ip" value="$(arg fcu_ip)"/>
-        <arg name="fcu_sys_id" value="$(arg fcu_sys_id)"/>
         <arg name="gcs_bridge" value="$(arg gcs_bridge)"/>
     </include>
 
@@ -58,9 +54,7 @@
     </node>
 
     <!-- main camera -->
-    <include file="$(find clover)/launch/main_camera.launch" if="$(arg main_camera)">
-        <arg name="simulator" value="$(arg simulator)"/>
-    </include>
+    <include file="$(find clover)/launch/main_camera.launch" if="$(arg main_camera)"/>
 
     <!-- rosbridge -->
     <include file="$(find rosbridge_server)/launch/rosbridge_websocket.launch" if="$(eval rosbridge or rc)"/>
@@ -69,16 +63,14 @@
     <node name="tf2_web_republisher" pkg="tf2_web_republisher" type="tf2_web_republisher" output="screen" if="$(arg rosbridge)"/>
 
     <!-- vl53l1x ToF rangefinder -->
-    <node name="rangefinder" pkg="vl53l1x" type="vl53l1x_node" output="screen" if="$(eval rangefinder_vl53l1x and not simulator)">
+    <node name="rangefinder" pkg="vl53l1x" type="vl53l1x_node" output="screen" if="$(arg rangefinder_vl53l1x)">
         <param name="frame_id" value="rangefinder"/>
         <param name="min_signal" value="0.4"/>
         <param name="pass_statuses" type="yaml" value="[0, 6, 7, 11]"/>
     </node>
 
     <!-- led strip -->
-    <include file="$(find clover)/launch/led.launch" if="$(arg led)">
-        <arg name="simulator" value="$(arg simulator)"/>
-    </include>
+    <include file="$(find clover)/launch/led.launch" if="$(arg led)"/>
 
     <!-- rc backend -->
     <node name="rc" pkg="clover" type="rc" output="screen" if="$(arg rc)" clear_params="true">
@@ -88,9 +80,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>
 </launch>

clover/launch/led.launch

@@ -2,12 +2,11 @@
     <arg name="ws281x" default="true"/>
     <arg name="led_effect" default="true"/>
     <arg name="led_notify" default="true"/>
-    <arg name="simulator" default="false"/>
 
     <!-- For additional help go to https://clover.coex.tech/led -->
 
     <!-- ws281x led strip driver -->
-    <node pkg="ws281x" name="led" type="ws281x_node" clear_params="true" output="screen" if="$(eval ws281x and not simulator)">
+    <node pkg="ws281x" name="led" type="ws281x_node" clear_params="true" output="screen" if="$(arg ws281x)">
         <param name="led_count" value="58"/>
         <param name="gpio_pin" value="21"/>
         <param name="brightness" value="64"/>

clover/launch/main_camera.launch

@@ -1,9 +1,8 @@
 <launch>
-    <!-- article about camera setup: https://clover.coex.tech/camera_setup -->
+    <!-- article about camera setup: https://clever.coex.tech/camera_frame -->
 
     <arg name="direction_z" default="down"/> <!-- direction the camera points: down, up -->
     <arg name="direction_y" default="backward"/> <!-- direction the camera cable points: backward, forward -->
-    <arg name="simulator" default="false"/>
 
     <node if="$(eval direction_z == 'down' and direction_y == 'backward')" pkg="tf2_ros" type="static_transform_publisher" name="main_camera_frame" args="0.05 0 -0.07 -1.5707963 0 3.1415926 base_link main_camera_optical"/>
     <node if="$(eval direction_z == 'down' and direction_y == 'forward')" pkg="tf2_ros" type="static_transform_publisher" name="main_camera_frame" args="0.05 0 -0.07 1.5707963 0 3.1415926 base_link main_camera_optical"/>
@@ -16,17 +15,17 @@
     <!-- <node pkg="tf2_ros" type="static_transform_publisher" name="main_camera_frame" args="0.05 0 -0.07 -1.5707963 0 3.1415926 base_link main_camera_optical"/> -->
 
     <!-- camera node -->
-    <node pkg="nodelet" type="nodelet" name="main_camera" args="load cv_camera/CvCameraNodelet nodelet_manager" clear_params="true" unless="$(arg simulator)">
+    <node pkg="nodelet" type="nodelet" name="main_camera" args="load cv_camera/CvCameraNodelet nodelet_manager" clear_params="true">
         <param name="device_path" value="/dev/video0"/> <!-- v4l2 device -->
         <param name="frame_id" value="main_camera_optical"/>
-        <param name="camera_info_url" value="file://$(find clover)/camera_info/fisheye_cam.yaml"/>
+        <param name="camera_info_url" value="file://$(find clover)/camera_info/fisheye_cam_320.yaml"/>
 
         <param name="rate" value="100"/> <!-- poll rate -->
         <param name="cv_cap_prop_fps" value="40"/> <!-- camera FPS -->
         <param name="capture_delay" value="0.02"/> <!-- approximate delay on frame retrieving -->
         <param name="rescale_camera_info" value="true"/> <!-- automatically rescale camera calibration info -->
 
-        <!-- camera resolution -->
+        <!-- camera resolution, NOTE: camera_info file should match it -->
         <param name="image_width" value="320"/>
         <param name="image_height" value="240"/>
     </node>

clover/launch/mavros.launch

@@ -1,7 +1,6 @@
 <launch>
     <arg name="fcu_conn" default="usb"/> <!-- options: usb, uart, tcp, udp, sitl -->
     <arg name="fcu_ip" default="127.0.0.1"/>
-    <arg name="fcu_sys_id" default="1"/>
     <arg name="gcs_bridge" default="tcp"/>
     <arg name="viz" default="true"/>
     <arg name="respawn" default="true"/>
@@ -20,9 +19,6 @@
         <!-- sitl since PX4 1.9.0 -->
         <param name="fcu_url" value="udp://@$(arg fcu_ip):14580" if="$(eval fcu_conn == 'sitl')"/>
 
-        <!-- set target_system_id -->
-        <param name="target_system_id" value="$(arg fcu_sys_id)" />
-
         <!-- 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')"/>

clover/package.xml

@@ -1,5 +1,5 @@
 <?xml version="1.0"?>
-<package format="2">
+<package format="3">
   <name>clover</name>
   <version>0.0.1</version>
   <description>The Clover package</description>
@@ -7,7 +7,7 @@
   <maintainer email="okalachev@gmail.com">Oleg Kalachev</maintainer>
   <license>MIT</license>
 
-  <url type="website">https://clover.coex.tech/</url>
+  <url type="website">https://clever.coex.tech/</url>
   <author email="okalachev@gmail.com">Oleg Kalachev</author>
   <author email="urpylka@gmail.com">Artem Smirnov</author>
 
@@ -37,8 +37,10 @@
   <depend>rosbridge_server</depend>
   <depend>web_video_server</depend>
   <depend>tf2_web_republisher</depend>
-  <depend>python-lxml</depend>
-  <exec_depend>python-pymavlink</exec_depend>
+  <depend condition="$ROS_PYTHON_VERSION == 2">python-lxml</depend>
+  <depend condition="$ROS_PYTHON_VERSION == 3">python3-lxml</depend>
+  <exec_depend condition="$ROS_PYTHON_VERSION == 2">python-pymavlink</exec_depend>
+  <exec_depend condition="$ROS_PYTHON_VERSION == 3">python-pymavlink</exec_depend>
   <!-- Use test_depend for packages you need only for testing: -->
   <!--   <test_depend>gtest</test_depend> -->
 

clover/requirements.txt

@@ -1,5 +1,5 @@
 flask==1.1.1
 docopt==0.6.2
-geopy==1.11.0
+geopy==1.20.0
 smbus2==0.3.0
-VL53L1X==0.0.5
+VL53L1X==0.0.4

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

@@ -1,4 +1,4 @@
-#!/usr/bin/env python
+#!/usr/bin/env python3
 # coding=utf-8
 
 # Copyright (C) 2018 Copter Express Technologies
@@ -138,7 +138,7 @@ def mavlink_exec(cmd, timeout=3.0):
         timeout=3,
         baudrate=0,
         count=len(cmd),
-        data=map(ord, cmd.ljust(70, '\0')))
+        data=[ord(c) for c in cmd.ljust(70, '\0')])
     msg.pack(link)
     ros_msg = mavlink.convert_to_rosmsg(msg)
     mavlink_pub.publish(ros_msg)
@@ -210,7 +210,7 @@ def check_fcu():
                     is_clover_firmware = True
 
         if not is_clover_firmware:
-            failure('not running Clover PX4 firmware, https://clover.coex.tech/firmware')
+            failure('not running Clover PX4 firmware, https://clever.coex.tech/firmware')
 
         est = get_param('SYS_MC_EST_GROUP')
         if est == 1:
@@ -250,11 +250,11 @@ def check_fcu():
         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')
+                failure('cell voltage is not available, https://clever.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)
+                    failure('incorrect cell voltage: %.2f V, https://clever.coex.tech/power', cell)
                 elif cell < 3.7:
                     failure('critically low cell voltage: %.2f V, recharge battery', cell)
         except rospy.ROSException:
@@ -609,7 +609,7 @@ def check_rangefinder():
 
 @check('Boot duration')
 def check_boot_duration():
-    output = subprocess.check_output('systemd-analyze')
+    output = subprocess.check_output('systemd-analyze').decode()
     r = re.compile(r'([\d\.]+)s\s*$', flags=re.MULTILINE)
     duration = float(r.search(output).groups()[0])
     if duration > 15:
@@ -620,7 +620,7 @@ def check_boot_duration():
 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); }'"
-    output = subprocess.check_output(CMD, shell=True)
+    output = subprocess.check_output(CMD, shell=True).decode()
     processes = output.split('\n')
     for process in processes:
         if not process:
@@ -636,7 +636,7 @@ def check_cpu_usage():
 def check_clover_service():
     try:
         output = subprocess.check_output('systemctl show -p ActiveState --value clover.service'.split(),
-                                         stderr=subprocess.STDOUT)
+                                         stderr=subprocess.STDOUT).decode()
     except subprocess.CalledProcessError as e:
         failure('systemctl returned %s: %s', e.returncode, e.output)
         return
@@ -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')
@@ -718,7 +718,7 @@ def check_network():
                 if ros_hostname in parts:
                     break
         else:
-            failure('not found %s in /etc/hosts, ROS will malfunction if network interfaces are down, https://clover.coex.tech/hostname', ros_hostname)
+            failure('not found %s in /etc/hosts, ROS will malfunction if network interfaces are down, https://clever.coex.tech/hostname', ros_hostname)
 
 
 @check('RPi health')
@@ -751,7 +751,7 @@ def check_rpi_health():
         # <parameter>=<value>
         # In case of `get_throttled`, <value> is a hexadecimal number
         # with some of the FLAGs OR'ed together
-        output = subprocess.check_output(['vcgencmd', 'get_throttled'])
+        output = subprocess.check_output(['vcgencmd', 'get_throttled']).decode()
     except OSError:
         failure('could not call vcgencmd binary; not a Raspberry Pi?')
         return

clover/src/simple_offboard.cpp

@@ -417,9 +417,8 @@ 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) {
+			position_msg.header.stamp = stamp;
 			position_pub.publish(position_msg);
 
 		} else {
@@ -491,7 +490,7 @@ inline void checkState()
 		throw std::runtime_error("State timeout, check mavros settings");
 
 	if (!state.connected)
-		throw std::runtime_error("No connection to FCU, https://clover.coex.tech/connection");
+		throw std::runtime_error("No connection to FCU, https://clever.coex.tech/connection");
 }
 
 #define ENSURE_FINITE(var) { if (!std::isfinite(var)) throw std::runtime_error(#var " argument cannot be NaN or Inf"); }
@@ -606,13 +605,12 @@ bool serve(enum setpoint_type_t sp_type, float x, float y, float z, float vx, fl
 
 		if (sp_type == POSITION || sp_type == NAVIGATE || sp_type == NAVIGATE_GLOBAL || sp_type == VELOCITY || sp_type == ATTITUDE) {
 			// destination point and/or yaw
-			PoseStamped ps;
+			static PoseStamped ps;
 			ps.header.frame_id = frame_id;
 			ps.header.stamp = stamp;
 			ps.pose.position.x = x;
 			ps.pose.position.y = y;
 			ps.pose.position.z = z;
-			ps.pose.orientation.w = 1.0; // Ensure quaternion is always valid
 
 			if (std::isnan(yaw)) {
 				setpoint_yaw_type = YAW_RATE;

clover/test/basic.py

@@ -1,4 +1,3 @@
-#!/usr/bin/env python
 import rospy
 import pytest
 from mavros_msgs.msg import State

clover/www/index.html

@@ -1,7 +1,7 @@
 <h1>Clover Drone Kit Tools</h1>
 
 <ul>
-	<li><a href="docs">View documentation</a> (snapshot of <a href="https://clover.coex.tech">clover.coex.tech</a>)</li>
+	<li><a href="docs">View documentation</a> (snapshot of <a href="https://clever.coex.tech">clever.coex.tech</a>)</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>
@@ -12,8 +12,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' });

clover_description/CMakeLists.txt

@@ -1,10 +0,0 @@
-cmake_minimum_required(VERSION 3.0)
-project(clover_description)
-
-find_package(catkin REQUIRED)
-
-catkin_package()
-
-install(DIRECTORY launch DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION})
-install(DIRECTORY meshes DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION})
-install(DIRECTORY urdf DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION})

clover_description/README.md

@@ -1,29 +0,0 @@
-# `clover_description` ROS package
-
-This package contains models and URDF descriptions for the Clover 4 drone. These descriptions can be used for Gazebo simulation environment.
-
-Note that in order to use these descriptions in Gazebo, you need to use the plugins from [PX4 `sitl_gazebo` package](https://github.com/PX4/sitl_gazebo) and `clover_simulation` package.
-
-## Usage
-
-The descriptions are provided as [`xacro`-enabled](https://wiki.ros.org/xacro) URDF files. A [`spawn_drone.launch`](launch/spawn_drone.launch) file that spawns the model in a running Gazebo instance is also provided for convenience.
-
-You may provide additional parameters for `spawn_drone.launch` as well:
-
-* `main_camera` (*boolean*, default: *true*) - controls whether the drone will have a downward-facing camera attached;
-* `rangefinder` (*boolean*, default: *true*) - controls whether the drone will have a downward-facing laser rangefinder attached;
-* `led` (*boolean*, default: *true*) - controls whether the drone will have a programmable LED strip (requires plugins from `clover_simulation`);
-* `gps` (*boolean*, default: *true*) - controls whether the drone will have a simulated GPS attached (requires plugins from `sitl_gazebo`);
-* `maintain_camera_rate` (*boolean*, default: *false*) - slow down the simultion to maintain camera publishing rate (internally changes the camera plugin from `libgazebo_ros_camera.so` to `libthrottling_camera.so` from [`clover_simulation`](../clover_simulation/README.md#throttling-camera-plugin-throttling_camera)).
-
-For example, in order to spawn a drone without a GPS module, you may use the following command:
-
-```bash
-roslaunch clover_description spawn_drone.launch gps:=false
-```
-
-## Tweaking
-
-By default, the `spawn_drone.launch` command will use the [`clover4.xacro` description file](urdf/clover/clover4.xacro). This is a "high-level" description of the drone, mainly used to attach additional sensors.
-
-The drone "physics" may be tweaked by changing the [`clover4_physics.xacro` file](urdf/clover/clover4_physics.xacro).

clover_description/launch/spawn_camera.launch

@@ -1,6 +0,0 @@
-<?xml version="1.0" encoding="UTF-8"?>
-<launch>
-    <arg name="cmd" default="$(find xacro)/xacro $(find clover_description)/urdf/sensors/rpi_cam.urdf.xacro"/>
-    <param command="$(arg cmd)" name="camera_description"/>
-    <node name="$(anon spawn)" output="screen" pkg="gazebo_ros" type="spawn_model" args="-urdf -param camera_description -model rpi_camera -package_to_model"/>
-</launch>

clover_description/launch/spawn_drone.launch

@@ -1,18 +0,0 @@
-<?xml version="1.0" encoding="UTF-8"?>
-<launch>
-    <!-- Toggleable model parameters -->
-    <!-- Main camera -->
-    <arg name="main_camera" default="true"/>
-    <!-- Slow simulation down to maintain camera rate -->
-    <arg name="maintain_camera_rate" default="false"/>
-    <arg name="rangefinder" default="true"/>
-    <arg name="led" default="true"/>
-    <arg name="gps" default="true"/>
-    <!-- Use physics parameters from CAD programs -->
-    <arg name="use_clover_physics" default="false"/>
-
-    <arg name="cmd" default="$(find xacro)/xacro $(find clover_description)/urdf/clover/clover4.xacro main_camera:=$(arg main_camera) rangefinder:=$(arg rangefinder) led:=$(arg led) gps:=$(arg gps) maintain_camera_rate:=$(arg maintain_camera_rate) use_clover_physics:=$(arg use_clover_physics)"/>
-    <param command="$(arg cmd)" name="drone_description"/>
-    <!-- Note: -package_to_model replaces all mentions of "package://" with "model://" in urdf URIs -->
-    <node name="$(anon spawn)" output="screen" pkg="gazebo_ros" type="spawn_model" args="-urdf -param drone_description -model clover -z 0.3"/> 
-</launch>

clover_description/launch/view_drone.launch

@@ -1,7 +0,0 @@
-<launch>
-  <arg name="model" default="$(find clover_description)/urdf/drones/clover4.xacro"/>
-
-  <param name="robot_description" command="$(find xacro)/xacro.py $(arg model)"/>
-
-  <node name="rviz" pkg="rviz" type="rviz" required="true"/>
-</launch>

clover_description/media/materials/scripts/guard_transparent.material

@@ -1,21 +0,0 @@
-material Polycarbonate
-{
-  technique
-  {
-    pass
-    {
-      scene_blend alpha_blend
-      depth_write off
-
-      ambient 0.57 0.531 0.444 1
-      diffuse 0.57 0.531 0.444 1
-      specular 0.5 0.5 0.5 1
-
-      texture_unit
-      {
-        colour_op_ex source1 src_current src_current 0 1 0
-        alpha_op_ex source1 src_manual src_current 0.72
-      }
-    }
-  }
-}

clover_description/package.xml

@@ -1,27 +0,0 @@
-<package format="2">
-  <name>clover_description</name>
-  <version>0.0.1</version>
-  <description>The clover_description package provides URDF models of the Clover series of quadcopters.</description>
-
-  <maintainer email="sfalexrog@gmail.com">Alexey Rogachevskiy</maintainer>
-
-  <author>Alexey Rogachevskiy</author>
-  <author>Andrey Ryabov</author>
-  <author>Arthur Golubtsov</author>
-  <author>Oleg Kalachev</author>
-  <author>Svyatoslav Zhuravlev</author>
-
-  <license>MIT</license>
-
-  <url type="website">https://github.com/CopterExpress/clover</url>
-  <url type="bugtracker">https://github.com/CopterExpress/clover/issues</url>
-
-  <buildtool_depend>catkin</buildtool_depend>
-  <depend>xacro</depend>
-  <exec_depend>gazebo_ros</exec_depend>
-  <exec_depend>gazebo_plugins</exec_depend>
-
-  <export>
-    <gazebo_ros gazebo_media_path="${prefix}"/>
-  </export>
-</package>

clover_description/urdf/camera_sensor.urdf.xacro

@@ -1,83 +0,0 @@
-<?xml version="1.0" encoding="UTF-8"?>
-<robot name="camera_sensor" xmlns:xacro="http://ros.org/wiki/xacro">
-  <xacro:macro name="camera_sensor_base"
-      params="name width height rate horizontal_fov frame_name k1 k2 k3 p1 p2 cx cy cx_prime ros_plugin_name min_rate window_size max_st_dev">
-    <gazebo reference="${name}_link">
-      <sensor type="camera" name="${name}">
-        <camera>
-          <horizontal_fov>${horizontal_fov}</horizontal_fov>
-          <image>
-            <format>B8G8R8</format>
-            <width>${width}</width>
-            <height>${height}</height>
-          </image>
-          <clip>
-            <near>0.02</near>
-            <far>1000</far>
-          </clip>
-          <distortion>
-            <k1>${k1}</k1>
-            <k2>${k2}</k2>
-            <k3>${k3}</k3>
-            <p1>${p1}</p1>
-            <p2>${p2}</p2>
-            <center>${(cx - 0.5)/ width} ${(cy - 0.5) / height}</center>
-          </distortion>
-        </camera>
-        <always_on>1</always_on>
-        <update_rate>${rate}</update_rate>
-        <visualize>1</visualize>
-        <plugin name="camera_plugin" filename="${ros_plugin_name}">
-          <alwaysOn>true</alwaysOn>
-          <imageTopicName>image_raw</imageTopicName>
-          <cameraTopicName>camera_info</cameraTopicName>
-          <updateRate>${rate}</updateRate>
-          <cameraName>${name}</cameraName>
-          <frameName>${frame_name}</frameName>
-          <CxPrime>${cx_prime}</CxPrime>
-          <Cx>${cx}</Cx>
-          <Cy>${cy}</Cy>
-          <distortionK1>${k1}</distortionK1>
-          <distortionK2>${k2}</distortionK2>
-          <distortionK3>${k3}</distortionK3>
-          <distortionT1>${p1}</distortionT1>
-          <distortionT2>${p2}</distortionT2>
-          <!-- throttling_camera specific options start here -->
-          <minUpdateRate>${min_rate}</minUpdateRate>
-          <windowSize>${window_size}</windowSize>
-          <maxStDev>${max_st_dev}</maxStDev>
-        </plugin>
-      </sensor>
-    </gazebo>
-  </xacro:macro>
-
-  <xacro:macro name="camera_sensor" params="name width height rate horizontal_fov k1:=0 k2:=0 k3:=0 p1:=0 p2:=0 do_throttling:=false">
-    <xacro:if value="${do_throttling}">
-      <xacro:property name="ros_plugin_name" value="libthrottling_camera.so"/>
-    </xacro:if>
-    <xacro:unless value="${do_throttling}">
-      <xacro:property name="ros_plugin_name" value="libgazebo_ros_camera.so"/>
-    </xacro:unless>
-    <xacro:camera_sensor_base
-      name="${name}"
-      width="${width}"
-      height="${height}"
-      rate="${rate}"
-      horizontal_fov="${horizontal_fov}"
-      frame_name="/${name}_optical"
-      k1="${k1}"
-      k2="${k2}"
-      k3="${k3}"
-      p1="${p1}"
-      p2="${p2}"
-      cx="${(width + 1.0)/2.0}"
-      cy="${(height + 1.0)/2.0}"
-      cx_prime="${(width + 1.0)/2.0}"
-      ros_plugin_name="${ros_plugin_name}"
-      min_rate="${rate * 0.95}"
-      window_size="${rate}"
-      max_st_dev="${3.0 / rate}"
-    />
-
-  </xacro:macro>
-</robot>

clover_description/urdf/clover/clover4.xacro

@@ -1,46 +0,0 @@
-<?xml version="1.0"?>
-<robot name="clover_camera" xmlns:xacro="http://ros.org/wiki/xacro">
-
-  <xacro:arg name="main_camera" default="true"/>
-  <xacro:arg name="rangefinder" default="true"/>
-  <xacro:arg name="led" default="true"/>
-  <xacro:arg name="gps" default="true"/>
-  <xacro:arg name="maintain_camera_rate" default="false"/>
-  <xacro:arg name="use_clover_physics" default="false"/>
-
-  <xacro:include filename="$(find clover_description)/urdf/clover/clover4_base.xacro" />
-  <xacro:include filename="$(find clover_description)/urdf/sensors/rpi_cam.urdf.xacro"/>
-  <xacro:include filename="$(find clover_description)/urdf/sensors/distance_sensor.urdf.xacro"/>
-  <xacro:include filename="$(find clover_description)/urdf/leds/led_strip.xacro"/>
-
-  <!-- Create camera plugin -->
-  <xacro:if value="$(arg main_camera)">
-    <xacro:rpi_cam name="main_camera" parent="base_link" x="0.055" y="0.0" z="-0.03" roll="0" pitch="${pi / 2}" yaw="0" width="320" height="240" rate="40" do_throttling="$(arg maintain_camera_rate)"/>
-  </xacro:if>
-
-  <!-- Create rangefinder plugin -->
-  <xacro:if value="$(arg rangefinder)">
-    <xacro:distance_sensor parent="base_link" x="0.0" y="0.0" z="-0.04" roll="0" pitch="${pi / 2}" yaw="0"/>
-  </xacro:if>
-
-  <!-- Instantiate LED strip -->
-  <xacro:if value="$(arg led)">
-    <xacro:led_strip
-      name="led"
-      parent="base_link"
-      radius="0.08"
-      bulb_radius="0.006"
-      led_count="58"
-      use_plugin="true"
-      z="-0.002"/>
-  </xacro:if>
-
-  <xacro:if value="$(arg gps)">
-    <!-- Instantiate gps plugin. -->
-    <xacro:gps_plugin_macro
-      namespace="${namespace}"
-      gps_noise="true"
-      />
-  </xacro:if>
-
-</robot>

clover_description/urdf/clover/clover4_base.xacro

@@ -1,183 +0,0 @@
-<?xml version="1.0"?>
-
-<robot name="clover" xmlns:xacro="http://ros.org/wiki/xacro">
-  <!-- Properties that can be assigned at build time as arguments.
-  Is there a reason not to make all properties arguments?
-  -->
-  <xacro:arg name='name' default='iris' />
-  <xacro:arg name='mavlink_addr' default='INADDR_ANY' />
-  <xacro:arg name='mavlink_udp_port' default='14560' />
-  <xacro:arg name='mavlink_tcp_port' default='4560' />
-  <xacro:arg name='serial_enabled' default='false' />
-  <xacro:arg name='serial_device' default='/dev/ttyACM0' />
-  <xacro:arg name='baudrate' default='921600' />
-  <xacro:arg name='qgc_addr' default='INADDR_ANY' />
-  <xacro:arg name='qgc_udp_port' default='14550' />
-  <xacro:arg name='sdk_addr' default='INADDR_ANY' />
-  <xacro:arg name='sdk_udp_port' default='14540' />
-  <xacro:arg name='hil_mode' default='false' />
-  <xacro:arg name='hil_state_level' default='false' />
-  <xacro:arg name='send_vision_estimation' default='false' />
-  <xacro:arg name='send_odometry' default='false' />
-  <xacro:arg name='enable_lockstep' default='true' />
-  <xacro:arg name='use_tcp' default='true' />
-  <xacro:arg name='vehicle_is_tailsitter' default='false' />
-  <xacro:arg name='visual_material' default='DarkGrey' />
-  <xacro:arg name='enable_mavlink_interface' default='true' />
-  <xacro:arg name='enable_wind' default='false' />
-  <!-- The following causes segfault with multiple vehicles if defaults to true!!! -->
-  <xacro:arg name='enable_ground_truth' default='false' />
-  <xacro:arg name='enable_logging' default='false' />
-  <xacro:arg name='log_file' default='iris' />
-
-  <!-- macros for gazebo plugins, sensors -->
-  <xacro:include filename="$(find clover_description)/urdf/component_snippets.urdf.xacro" />
-
-  <!-- Instantiate iris "mechanics" -->
-  <xacro:include filename="$(find clover_description)/urdf/clover/clover4_physics.xacro" />
-
-  <xacro:if value="$(arg enable_wind)">
-    <xacro:wind_plugin_macro
-        namespace="${namespace}"
-        wind_direction="0 0 1"
-        wind_force_mean="0.7"
-        xyz_offset="1 0 0"
-        wind_gust_direction="0 0 0"
-        wind_gust_duration="0"
-        wind_gust_start="0"
-        wind_gust_force_mean="0"
-        />
-  </xacro:if>
-
-  <!-- Instantiate magnetometer plugin. -->
-  <xacro:magnetometer_plugin_macro
-    namespace="${namespace}"
-    pub_rate="20"
-    noise_density="0.0004"
-    random_walk="0.0000064"
-    bias_correlation_time="600"
-    mag_topic="/mag"
-    >
-  </xacro:magnetometer_plugin_macro>
-
-  <!-- Instantiate barometer plugin. -->
-  <xacro:barometer_plugin_macro
-    namespace="${namespace}"
-    pub_rate="10"
-    baro_topic="/baro"
-    >
-  </xacro:barometer_plugin_macro>
-
-  <xacro:if value="$(arg enable_mavlink_interface)">
-  <!-- Instantiate mavlink telemetry interface. -->
-  <xacro:mavlink_interface_macro
-    namespace="${namespace}"
-    imu_sub_topic="/imu"
-    gps_sub_topic="/gps"
-    mag_sub_topic="/mag"
-    baro_sub_topic="/baro"
-    mavlink_addr="$(arg mavlink_addr)"
-    mavlink_udp_port="$(arg mavlink_udp_port)"
-    mavlink_tcp_port="$(arg mavlink_tcp_port)"
-    serial_enabled="$(arg serial_enabled)"
-    serial_device="$(arg serial_device)"
-    baudrate="$(arg baudrate)"
-    qgc_addr="$(arg qgc_addr)"
-    qgc_udp_port="$(arg qgc_udp_port)"
-    sdk_addr="$(arg sdk_addr)"
-    sdk_udp_port="$(arg sdk_udp_port)"
-    hil_mode="$(arg hil_mode)"
-    hil_state_level="$(arg hil_state_level)"
-    vehicle_is_tailsitter="$(arg vehicle_is_tailsitter)"
-    send_vision_estimation="$(arg send_vision_estimation)"
-    send_odometry="$(arg send_odometry)"
-    enable_lockstep="$(arg enable_lockstep)"
-    use_tcp="$(arg use_tcp)"
-    >
-  </xacro:mavlink_interface_macro>
-  </xacro:if>
-
-  <!-- Mount an ADIS16448 IMU. -->
-  <xacro:imu_plugin_macro
-    namespace="${namespace}"
-    imu_suffix=""
-    parent_link="base_link"
-    imu_topic="/imu"
-    mass_imu_sensor="0.015"
-    gyroscope_noise_density="0.0003394"
-    gyroscopoe_random_walk="0.000038785"
-    gyroscope_bias_correlation_time="1000.0"
-    gyroscope_turn_on_bias_sigma="0.0087"
-    accelerometer_noise_density="0.004"
-    accelerometer_random_walk="0.006"
-    accelerometer_bias_correlation_time="300.0"
-    accelerometer_turn_on_bias_sigma="0.1960"
-  >
-    <inertia ixx="0.00001" ixy="0.0" ixz="0.0" iyy="0.00001" iyz="0.0" izz="0.00001" />
-    <origin xyz="0 0 0" rpy="0 0 0" />
-  </xacro:imu_plugin_macro>
-
-  <xacro:if value="$(arg enable_ground_truth)">
-    <!-- Mount an IMU providing ground truth. -->
-    <xacro:imu_plugin_macro
-      namespace="${namespace}"
-      imu_suffix="gt"
-      parent_link="base_link"
-      imu_topic="ground_truth/imu"
-      mass_imu_sensor="0.00001"
-      gyroscope_noise_density="0.0"
-      gyroscopoe_random_walk="0.0"
-      gyroscope_bias_correlation_time="1000.0"
-      gyroscope_turn_on_bias_sigma="0.0"
-      accelerometer_noise_density="0.0"
-      accelerometer_random_walk="0.0"
-      accelerometer_bias_correlation_time="300.0"
-      accelerometer_turn_on_bias_sigma="0.0"
-    >
-      <inertia ixx="0.00001" ixy="0.0" ixz="0.0" iyy="0.00001" iyz="0.0" izz="0.00001" />
-      <origin xyz="0 0 0" rpy="0 0 0" />
-    </xacro:imu_plugin_macro>
-
-    <!-- Mount a generic odometry sensor providing ground truth. -->
-    <xacro:odometry_plugin_macro
-      namespace="${namespace}/ground_truth"
-      odometry_sensor_suffix="gt"
-      parent_link="base_link"
-      pose_topic="pose"
-      pose_with_covariance_topic="pose_with_covariance"
-      position_topic="position"
-      transform_topic="transform"
-      odometry_topic="odometry"
-      parent_frame_id="world"
-      mass_odometry_sensor="0.00001"
-      measurement_divisor="1"
-      measurement_delay="0"
-      unknown_delay="0.0"
-      noise_normal_position="0 0 0"
-      noise_normal_quaternion="0 0 0"
-      noise_normal_linear_velocity="0 0 0"
-      noise_normal_angular_velocity="0 0 0"
-      noise_uniform_position="0 0 0"
-      noise_uniform_quaternion="0 0 0"
-      noise_uniform_linear_velocity="0 0 0"
-      noise_uniform_angular_velocity="0 0 0"
-      enable_odometry_map="false"
-      odometry_map=""
-      image_scale=""
-    >
-      <inertia ixx="0.00001" ixy="0.0" ixz="0.0" iyy="0.00001" iyz="0.0" izz="0.00001" /> <!-- [kg.m^2] [kg.m^2] [kg.m^2] [kg.m^2] [kg.m^2] [kg.m^2] -->
-      <origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0" />
-    </xacro:odometry_plugin_macro>
-  </xacro:if>
-
-  <xacro:if value="$(arg enable_logging)">
-    <!-- Instantiate a logger -->
-    <xacro:bag_plugin_macro
-      namespace="${namespace}"
-      bag_file="$(arg log_file)"
-      rotor_velocity_slowdown_sim="${rotor_velocity_slowdown_sim}"
-    >
-    </xacro:bag_plugin_macro>
-  </xacro:if>
-
-</robot>

clover_description/urdf/clover/clover4_macros.xacro

@@ -1,235 +0,0 @@
-<?xml version="1.0"?>
-<!--
-  Copyright 2015 Fadri Furrer, ASL, ETH Zurich, Switzerland
-  Copyright 2015 Michael Burri, ASL, ETH Zurich, Switzerland
-  Copyright 2015 Mina Kamel, ASL, ETH Zurich, Switzerland
-  Copyright 2015 Janosch Nikolic, ASL, ETH Zurich, Switzerland
-  Copyright 2015 Markus Achtelik, ASL, ETH Zurich, Switzerland
-
-  Licensed under the Apache License, Version 2.0 (the "License");
-  you may not use this file except in compliance with the License.
-  You may obtain a copy of the License at
-
-  http://www.apache.org/licenses/LICENSE-2.0
-
-  Unless required by applicable law or agreed to in writing, software
-  distributed under the License is distributed on an "AS IS" BASIS,
-  WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
-  See the License for the specific language governing permissions and
-  limitations under the License.
--->
-
-<!--
-  Clover 4 base model, collision boxes, and other fun stuff. Derived heavily from multirotor_base.xacro
-  from PX4 simulation config files.
--->
-
-<robot xmlns:xacro="http://ros.org/wiki/xacro">
-  <!-- Main multirotor link -->
-  <!-- We still allow specifying a different mass and inertia matrix.
-       Body width and height are allowed as parameters, but are ignored for the most part.
-       Color is not applicable, since the model has textures and is a compound thingy anyway.
-  -->
-  <xacro:macro name="clover4_base_macro"
-    params="robot_namespace mass body_width body_height *inertia">
-    <link name="base_link"></link>
-
-    <!-- Note: For some reason this file relies on base_link_inertia becoming base_link.
-         I don't even want to know.
-         -sfalexrog
-    -->
-    <joint name="base_joint" type="fixed">
-      <origin xyz="0 0 0" rpy="0 0 0" />
-      <parent link="base_link" />
-      <child link="base_link_inertia" />
-    </joint>
-
-    <link name="base_link_inertia">
-      <inertial>
-        <mass value="${mass}" />  <!-- [kg] -->
-        <origin xyz="0 0 0" />
-        <xacro:insert_block name="inertia" />
-      </inertial>
-      <visual name="body">
-        <origin xyz="0 0 0.025" rpy="0 0 0" />
-        <geometry>
-          <!-- Note: Texture files are expected to be in the same directory as the model -->
-          <mesh filename="package://clover_description/meshes/clover4/clover_body_solid.dae"
-            scale="1 1 1"/>
-        </geometry>
-      </visual>
-      <!-- Now, a person with some experience in building urdf/xacro files would say something along the lines of:
-           "Hey, a link may have multiple visual tags, each with its own material!"
-           Hear my answer, o experienced one. First, you are absolutely correct, you may add multiple visual
-           elements to a single link. Second, once you try to put some transparency on your visual elements,
-           you'll have it vanish into thin fucking air - Gazebo (as of version 9!) gives fuck all about your
-           "color" attributes and whatnot. Third, oh, you want to reference the visual element from your
-           "gazebo" tag down the line? Well fuck you, buddy, because you can only reference links and joints!
-           So yeah, good luck improving this.
-           - sfalexrog
-      -->
-      <collision>
-        <origin xyz="0 0 0" rpy="0 0 0" />
-        <geometry>
-          <box size="${body_width} ${body_width} ${body_height}" /> <!-- [m] [m] [m] -->
-        </geometry>
-      </collision>
-    </link>
-
-    <!-- This link is here for a single reason: to draw transparent parts (oh, and also to be the bane of my existence).
-         I'm going to leave helpful comments for the poor soul who's going to attempt to improve this file.
-        -sfalexrog
-    -->
-    <link name="base_guard_link">
-      <!-- So we've established we actually need a link. Now, if you look at the SDF spec for some reason
-           (much like I did), you'll notice you don't really need much for a link to exist. And that is a
-           big fucking lie. Sure, go ahead, create a link without inertial or collision properties,
-           see how far that gets you (spoiler: it will get you fucking nowhere, because Gazebo will
-           devour your puny link and all that it stands for).
-           - sfalexrog
-      -->
-      <inertial>
-        <!-- We give an oh-so-tiny mass to the link to avoid it being destroyed by Gazebo -->
-        <mass value="0.015"/> <!-- [kg] -->
-        <origin xyz="0 0 0" rpy="0 0 0"/>
-        <!-- We also give it some inertia, so that the physics engine does not go crazy
-             (and to avoid it being devoured by Gazebo)
-        -->
-        <inertia 
-          ixx="${1/12 * 0.015 * 0.01 * 0.01}"
-          iyy="${1/12 * 0.015 * 0.01 * 0.01}"
-          izz="${1/12 * 0.015 * 0.01 * 0.01}"
-          ixy="0.0" ixz="0.0" iyz="0.0"
-        />
-      </inertial>
-      <!-- We may want to have proper collision tucked away here at some point, though -->
-      <collision>
-        <origin xyz="0 0 0" rpy="0 0 0" />
-        <geometry>
-          <box size="0.01 0.01 0.01" /> <!-- [m] [m] [m] -->
-        </geometry>
-      </collision>
-      <visual>
-        <!-- This origin should be the same as in "base_link_inertia" visuals -->
-        <origin xyz="0 0 0.025" rpy="0 0 0"/>
-        <geometry>
-          <mesh filename="package://clover_description/meshes/clover4/clover_guards_transparent.dae"
-            scale="1 1 1"/>
-        </geometry>
-        <!-- Let me reiterate that you can't put the "material" tag to any effect -->
-      </visual>
-    </link>
-
-    <!-- Now, the last thing to do is to attach the base_guard_link to base_link. Since
-         base_guard_link has no meaning by itself, it must be fixed rigidly to base_link.
-         Except Gazebo absolutely loves "optimizing away" your fixed joints and all
-         child links with them! How do we deal with that nonsense? Read on to find out!
-         - sfalexrog
-    -->
-    <joint name="base_guard_joint" type="fixed">
-      <origin xyz="0 0 0" rpy="0 0 0"/>
-      <parent link="base_link" />
-      <child link="base_guard_link" />
-    </joint>
-
-    <!-- attach multirotor_base_plugin to the base_link -->
-    <gazebo>
-      <plugin filename="libgazebo_multirotor_base_plugin.so" name="rosbag">
-        <robotNamespace>${robot_namespace}</robotNamespace>
-        <linkName>base_link</linkName>
-        <rotorVelocitySlowdownSim>${rotor_velocity_slowdown_sim}</rotorVelocitySlowdownSim>
-      </plugin>
-    </gazebo>
-    <!-- And here's what you've been waiting for: the way to save a fixed joint from
-         Gazebo's wrath! You plead for it to save your joint, and it might answer
-        (depending on the version, apparently).
-        Note that it is done in other places as well.
-        - sfalexrog
-     -->
-    <gazebo reference="base_guard_joint">
-      <disableFixedJointLumping>true</disableFixedJointLumping>
-      <preserveFixedJoint>true</preserveFixedJoint>
-    </gazebo>
-    <!-- ...and here's another absolutely terrible idea: you can only set a material for
-         the whole link, not for its part. And that's incompatible with URDF's material
-         definition. Yeah.
-         - sfalexrog
-    -->
-    <gazebo reference="base_guard_link">
-      <material>Polycarbonate</material>
-    </gazebo>
-    <gazebo reference="base_link">
-      <kp>10000.0</kp>
-      <kd>10.0</kd>
-      <maxVel>10.0</maxVel>
-      <minDepth>0.001</minDepth>
-   </gazebo>
-  </xacro:macro>
-
-  <!-- Rotor joint and link, modified for Clover purposes -->
-  <xacro:macro name="vertical_clover_rotor"
-    params="robot_namespace suffix direction motor_constant moment_constant parent mass_rotor radius_rotor time_constant_up time_constant_down max_rot_velocity motor_number rotor_drag_coefficient rolling_moment_coefficient mesh *origin *inertia">
-    <joint name="rotor_${motor_number}_joint" type="continuous">
-      <xacro:insert_block name="origin" />
-      <axis xyz="0 0 1" />
-      <!-- TODO(ff): not currently set because it's not yet supported -->
-      <!-- <limit effort="2000" velocity="${max_rot_velocity}" /> -->
-      <parent link="${parent}" />
-      <child link="rotor_${motor_number}" />
-
-    </joint>
-    <!-- TODO(ff): not currently set because it's not yet supported -->
-    <!-- <gazebo reference="rotor_${motor_number}_joint"> <axis> <xyz>0 0 1</xyz>
-      <limit> <velocity> ${max_rot_velocity} </velocity> </limit> </axis> </gazebo> -->
-    <link name="rotor_${motor_number}">
-      <inertial>
-        <mass value="${mass_rotor}" /> <!-- [kg] -->
-        <xacro:insert_block name="inertia" />
-      </inertial>
-      <visual>
-        <geometry>
-          <mesh filename="package://clover_description/meshes/${mesh}"
-            scale="1 1 1" />
-        </geometry>
-      </visual>
-      <collision>
-        <geometry>
-          <cylinder length="0.005" radius="${radius_rotor}" /> <!-- [m] -->
-        </geometry>
-      </collision>
-    </link>
-    <gazebo>
-      <plugin name="${suffix}_motor_model" filename="libgazebo_motor_model.so">
-        <robotNamespace>${robot_namespace}</robotNamespace>
-        <jointName>rotor_${motor_number}_joint</jointName>
-        <linkName>rotor_${motor_number}</linkName>
-        <turningDirection>${direction}</turningDirection>
-        <timeConstantUp>${time_constant_up}</timeConstantUp>
-        <timeConstantDown>${time_constant_down}</timeConstantDown>
-        <maxRotVelocity>${max_rot_velocity}</maxRotVelocity>
-        <motorConstant>${motor_constant}</motorConstant>
-        <momentConstant>${moment_constant}</momentConstant>
-        <commandSubTopic>/gazebo/command/motor_speed</commandSubTopic>
-        <motorNumber>${motor_number}</motorNumber>
-        <rotorDragCoefficient>${rotor_drag_coefficient}</rotorDragCoefficient>
-        <rollingMomentCoefficient>${rolling_moment_coefficient}</rollingMomentCoefficient>
-        <motorSpeedPubTopic>/motor_speed/${motor_number}</motorSpeedPubTopic>
-        <rotorVelocitySlowdownSim>${rotor_velocity_slowdown_sim}</rotorVelocitySlowdownSim>
-        <!--
-        <gazebo_joint_control_pid>
-          <p>0.1</p>
-          <i>0</i>
-          <d>0</d>
-          <iMax>0</iMax>
-          <iMin>0</iMin>
-          <cmdMax>3</cmdMax>
-          <cmdMin>-3</cmdMin>
-        </gazebo_joint_control_pid>
-      -->
-      </plugin>
-    </gazebo>
-    <!-- <gazebo reference="rotor_${motor_number}">
-      <material>Gazebo/${color}</material>
-    </gazebo> -->
-  </xacro:macro>
-</robot>

clover_description/urdf/clover/clover4_physics.xacro

@@ -1,178 +0,0 @@
-<?xml version="1.0"?>
-
-<!-- Typical physics values for the Clover drone.
-     NB: Right now the values are wrong, and taken from the 3DR Iris drone description.
-     This is subject to change as correct values are calculated.
-
--->
-
-<robot name="iris" xmlns:xacro="http://ros.org/wiki/xacro">
-  <!-- Properties -->
-  <xacro:property name="namespace" value="" />
-  <xacro:property name="rotor_velocity_slowdown_sim" value="10" />
-  <xacro:if value="$(arg use_clover_physics)">
-    <xacro:property name="mass" value="0.7" /> <!-- [kg] -->
-  </xacro:if>
-  <xacro:unless value="$(arg use_clover_physics)">
-    <xacro:property name="mass" value="1.5" /> <!-- [kg] -->
-  </xacro:unless>
-  <xacro:property name="body_width" value="0.35" /> <!-- [m] -->
-  <xacro:property name="body_height" value="0.124" /> <!-- [m] -->
-  <xacro:property name="mass_rotor" value="0.01" /> <!-- [kg] -->
-  <xacro:property name="arm_length_front_x" value="0.083" /> <!-- [m] -->
-  <xacro:property name="arm_length_back_x" value="0.083" /> <!-- [m] -->
-  <xacro:property name="arm_length_front_y" value="0.083" /> <!-- [m] -->
-  <xacro:property name="arm_length_back_y" value="0.083" /> <!-- [m] -->
-  <xacro:property name="rotor_offset_top" value="0.026" /> <!-- [m] -->
-  <xacro:property name="radius_rotor" value="0.06" /> <!-- [m] -->
-  <xacro:property name="motor_constant" value="8.54858e-06" /> <!-- [kg.m/s^2] -->
-  <xacro:property name="moment_constant" value="0.06" /> <!-- [m] -->
-  <xacro:property name="time_constant_up" value="0.0125" /> <!-- [s] -->
-  <xacro:property name="time_constant_down" value="0.025" /> <!-- [s] -->
-  <xacro:property name="max_rot_velocity" value="1100" /> <!-- [rad/s] -->
-  <xacro:property name="sin30" value="0.5" />
-  <xacro:property name="cos30" value="0.866025403784" />
-  <xacro:property name="sqrt2" value="1.4142135623730951" />
-  <xacro:property name="rotor_drag_coefficient" value="1.75e-04" />
-  <xacro:property name="rolling_moment_coefficient" value="0.000001" />
-  <xacro:property name="color" value="$(arg visual_material)" />
-
-  <!-- Property Blocks -->
-  <!-- Clover body inertia -->
-  <!-- Values from CAD program -->
-  <xacro:if value="$(arg use_clover_physics)">
-    <xacro:property name="body_inertia">
-      <inertia
-      ixx="0.0347563"
-      iyy="0.0458929"
-      izz="0.0977"
-      ixy="0.0" ixz="0.0" iyz="0.0" /> <!-- [kg.m^2] [kg.m^2] [kg.m^2] [kg.m^2] [kg.m^2] [kg.m^2] -->
-    </xacro:property>
-  </xacro:if>
-  <!-- Box-like inertia -->
-  <xacro:unless value="$(arg use_clover_physics)">
-    <xacro:property name="body_inertia">
-      <inertia 
-      ixx="${1/12 * mass * (body_height * body_height + body_width * body_width)}"
-      iyy="${1/12 * mass * (body_height * body_height + body_width * body_width)}"
-      izz="${1/12 * mass * (body_width * body_width + body_width * body_width)}"    
-      ixy="0.0" ixz="0.0" iyz="0.0" /> <!-- [kg.m^2] [kg.m^2] [kg.m^2] [kg.m^2] [kg.m^2] [kg.m^2] -->
-    </xacro:property>
-  </xacro:unless>
-
-  <!-- inertia of a single rotor -->
-  <!-- Values from CAD program -->
-  <xacro:if value="$(arg use_clover_physics)">
-    <xacro:property name="rotor_inertia">
-      <inertia
-      ixx="${1.375 * 0.001 * 0.001 * rotor_velocity_slowdown_sim}"
-      iyy="${2.153 * 0.001 * 0.001 * rotor_velocity_slowdown_sim}"
-      izz="${1.375 * 0.001 * 0.001 * rotor_velocity_slowdown_sim}"
-      ixy="0.0" ixz="0.0"  iyz="0.0" /> <!-- [kg.m^2] [kg.m^2] [kg.m^2] [kg.m^2] [kg.m^2] [kg.m^2] -->
-    </xacro:property>
-  </xacro:if>
-
-  <!-- Values for a cuboid (cuboid. Height=3mm, width=15mm) -->
-  <xacro:unless value="$(arg use_clover_physics)">
-    <xacro:property name="rotor_inertia">
-      <inertia
-      ixx="${1/12 * mass_rotor * (0.015 * 0.015 + 0.003 * 0.003) * rotor_velocity_slowdown_sim}"
-      iyy="${1/12 * mass_rotor * (4 * radius_rotor * radius_rotor + 0.003 * 0.003) * rotor_velocity_slowdown_sim}"
-      izz="${1/12 * mass_rotor * (4 * radius_rotor * radius_rotor + 0.015 * 0.015) * rotor_velocity_slowdown_sim}"
-      ixy="0.0" ixz="0.0"  iyz="0.0" /> <!-- [kg.m^2] [kg.m^2] [kg.m^2] [kg.m^2] [kg.m^2] [kg.m^2] -->
-    </xacro:property>
-  </xacro:unless>
-
-  <!-- Included URDF Files -->
-  <xacro:include filename="$(find clover_description)/urdf/clover/clover4_macros.xacro" />
-
-  <!-- Instantiate multirotor_base_macro once -->
-  <xacro:clover4_base_macro
-    robot_namespace="${namespace}"
-    mass="${mass}"
-    body_width="${body_width}"
-    body_height="${body_height}"
-    >
-    <xacro:insert_block name="body_inertia" />
-  </xacro:clover4_base_macro>
-
-  <!-- Instantiate rotors -->
-  <xacro:vertical_clover_rotor
-    robot_namespace="${namespace}"
-    suffix="front_right"
-    direction="ccw"
-    motor_constant="${motor_constant}"
-    moment_constant="${moment_constant}"
-    parent="base_link"
-    mass_rotor="${mass_rotor}"
-    radius_rotor="${radius_rotor}"
-    time_constant_up="${time_constant_up}"
-    time_constant_down="${time_constant_down}"
-    max_rot_velocity="${max_rot_velocity}"
-    motor_number="0"
-    rotor_drag_coefficient="${rotor_drag_coefficient}"
-    rolling_moment_coefficient="${rolling_moment_coefficient}"
-    mesh="clover4/prop_ccw.dae">
-    <origin xyz="${arm_length_front_x} -${arm_length_front_y} ${rotor_offset_top}" rpy="0 0 0" />
-    <xacro:insert_block name="rotor_inertia" />
-  </xacro:vertical_clover_rotor>
-
-  <xacro:vertical_clover_rotor
-    robot_namespace="${namespace}"
-    suffix="back_left"
-    direction="ccw"
-    motor_constant="${motor_constant}"
-    moment_constant="${moment_constant}"
-    parent="base_link"
-    mass_rotor="${mass_rotor}"
-    radius_rotor="${radius_rotor}"
-    time_constant_up="${time_constant_up}"
-    time_constant_down="${time_constant_down}"
-    max_rot_velocity="${max_rot_velocity}"
-    motor_number="1"
-    rotor_drag_coefficient="${rotor_drag_coefficient}"
-    rolling_moment_coefficient="${rolling_moment_coefficient}"
-    mesh="clover4/prop_ccw.dae">
-    <origin xyz="-${arm_length_back_x} ${arm_length_back_y} ${rotor_offset_top}" rpy="0 0 0" />
-    <xacro:insert_block name="rotor_inertia" />
-  </xacro:vertical_clover_rotor>
-
-  <xacro:vertical_clover_rotor robot_namespace="${namespace}"
-    suffix="front_left"
-    direction="cw"
-    motor_constant="${motor_constant}"
-    moment_constant="${moment_constant}"
-    parent="base_link"
-    mass_rotor="${mass_rotor}"
-    radius_rotor="${radius_rotor}"
-    time_constant_up="${time_constant_up}"
-    time_constant_down="${time_constant_down}"
-    max_rot_velocity="${max_rot_velocity}"
-    motor_number="2"
-    rotor_drag_coefficient="${rotor_drag_coefficient}"
-    rolling_moment_coefficient="${rolling_moment_coefficient}"
-    mesh="clover4/prop_cw.dae">
-    <origin xyz="${arm_length_front_x} ${arm_length_front_y} ${rotor_offset_top}" rpy="0 0 0" />
-    <xacro:insert_block name="rotor_inertia" />
-  </xacro:vertical_clover_rotor>
-
-  <xacro:vertical_clover_rotor robot_namespace="${namespace}"
-    suffix="back_right"
-    direction="cw"
-    motor_constant="${motor_constant}"
-    moment_constant="${moment_constant}"
-    parent="base_link"
-    mass_rotor="${mass_rotor}"
-    radius_rotor="${radius_rotor}"
-    time_constant_up="${time_constant_up}"
-    time_constant_down="${time_constant_down}"
-    max_rot_velocity="${max_rot_velocity}"
-    motor_number="3"
-    rotor_drag_coefficient="${rotor_drag_coefficient}"
-    rolling_moment_coefficient="${rolling_moment_coefficient}"
-    mesh="clover4/prop_cw.dae">
-    <origin xyz="-${arm_length_back_x} -${arm_length_back_y} ${rotor_offset_top}" rpy="0 0 0" />
-    <xacro:insert_block name="rotor_inertia" />
-  </xacro:vertical_clover_rotor>
-
-</robot>

clover_description/urdf/component_snippets.urdf.xacro

@@ -1,613 +0,0 @@
-<?xml version="1.0"?>
-<!--
-  Copyright 2015 Fadri Furrer, ASL, ETH Zurich, Switzerland
-  Copyright 2015 Michael Burri, ASL, ETH Zurich, Switzerland
-  Copyright 2015 Mina Kamel, ASL, ETH Zurich, Switzerland
-  Copyright 2015 Janosch Nikolic, ASL, ETH Zurich, Switzerland
-  Copyright 2015 Markus Achtelik, ASL, ETH Zurich, Switzerland
-
-  Licensed under the Apache License, Version 2.0 (the "License");
-  you may not use this file except in compliance with the License.
-  You may obtain a copy of the License at
-
-  http://www.apache.org/licenses/LICENSE-2.0
-
-  Unless required by applicable law or agreed to in writing, software
-  distributed under the License is distributed on an "AS IS" BASIS,
-  WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
-  See the License for the specific language governing permissions and
-  limitations under the License.
--->
-
-<robot xmlns:xacro="http://ros.org/wiki/xacro">
-  <!-- Macro to add logging to a bag file. -->
-  <xacro:macro name="bag_plugin_macro"
-    params="namespace bag_file rotor_velocity_slowdown_sim">
-    <gazebo>
-      <plugin filename="librotors_gazebo_bag_plugin.so" name="rosbag">
-        <robotNamespace>${namespace}</robotNamespace>
-        <bagFileName>${bag_file}</bagFileName>
-        <linkName>base_link</linkName>
-        <frameId>base_link</frameId>
-        <commandAttitudeThrustSubTopic>command/attitude</commandAttitudeThrustSubTopic>
-        <commandAttitudeThrustPubTopic>command/attitude</commandAttitudeThrustPubTopic>
-        <rotorVelocitySlowdownSim>${rotor_velocity_slowdown_sim}</rotorVelocitySlowdownSim>
-      </plugin>
-    </gazebo>
-  </xacro:macro>
-
-  <!-- Macro to add a camera. -->
-  <xacro:macro name="camera_macro"
-    params="namespace parent_link camera_suffix frame_rate
-      horizontal_fov image_width image_height image_format min_distance
-      max_distance noise_mean noise_stddev enable_visual *cylinder *origin">
-    <link name="${namespace}/camera_${camera_suffix}_link">
-      <collision>
-        <origin xyz="0 0 0" rpy="0 0 0" />
-        <geometry>
-          <xacro:insert_block name="cylinder" />
-        </geometry>
-      </collision>
-      <xacro:if value="${enable_visual}">
-        <visual>
-          <origin xyz="0 0 0" rpy="0 1.57079632679 0" />
-          <geometry>
-            <xacro:insert_block name="cylinder" />
-          </geometry>
-          <material name="red" />
-        </visual>
-      </xacro:if>
-      <inertial>
-        <mass value="1e-5" />
-        <origin xyz="0 0 0" rpy="0 0 0" />
-        <inertia ixx="1e-6" ixy="0" ixz="0" iyy="1e-6" iyz="0" izz="1e-6" />
-      </inertial>
-    </link>
-    <joint name="${namespace}/camera_${camera_suffix}_joint" type="fixed">
-      <xacro:insert_block name="origin" />
-      <parent link="${parent_link}" />
-      <child link="${namespace}/camera_${camera_suffix}_link" />
-    </joint>
-    <gazebo reference="${namespace}/camera_${camera_suffix}_link">
-      <sensor type="camera" name="${namespace}_camera_${camera_suffix}">
-        <update_rate>${frame_rate}</update_rate>
-        <camera name="head">
-          <horizontal_fov>${horizontal_fov}</horizontal_fov>
-          <image>
-            <width>${image_width}</width>
-            <height>${image_height}</height>
-            <format>${image_format}</format>
-          </image>
-          <clip>
-            <near>${min_distance}</near>
-            <far>${max_distance}</far>
-          </clip>
-          <noise>
-            <type>gaussian</type>
-            <!-- Noise is sampled independently per pixel on each frame.
-                 That pixel's noise value is added to each of its color
-                 channels, which at that point lie in the range [0,1]. -->
-            <mean>${noise_mean}</mean>
-            <stddev>${noise_stddev}</stddev>
-          </noise>
-        </camera>
-        <plugin name="${namespace}_camera_${camera_suffix}_controller" filename="libgazebo_ros_camera.so">
-          <robotNamespace>${namespace}</robotNamespace>
-          <alwaysOn>true</alwaysOn>
-          <updateRate>${frame_rate}</updateRate>
-          <cameraName>camera_${camera_suffix}</cameraName>
-          <imageTopicName>image_raw</imageTopicName>
-          <cameraInfoTopicName>camera_info</cameraInfoTopicName>
-          <frameName>camera_${camera_suffix}_link</frameName>
-          <hackBaseline>0.0</hackBaseline>
-          <distortionK1>0.0</distortionK1>
-          <distortionK2>0.0</distortionK2>
-          <distortionK3>0.0</distortionK3>
-          <distortionT1>0.0</distortionT1>
-          <distortionT2>0.0</distortionT2>
-        </plugin>
-      </sensor>
-    </gazebo>
-  </xacro:macro>
-
-  <!-- Macro to add the controller interface. -->
-  <xacro:macro name="controller_plugin_macro" params="namespace imu_sub_topic">
-    <gazebo>
-      <plugin name="controller_interface" filename="libgazebo_controller_interface.so">
-        <robotNamespace>${namespace}</robotNamespace>
-        <commandAttitudeThrustSubTopic>/command/attitude</commandAttitudeThrustSubTopic>
-        <commandRateThrustSubTopic>/command/rate</commandRateThrustSubTopic>
-        <commandMotorSpeedSubTopic>/command/motor_speed</commandMotorSpeedSubTopic>
-        <imuSubTopic>/${imu_sub_topic}</imuSubTopic>
-        <motorSpeedCommandPubTopic>/gazebo/command/motor_speed</motorSpeedCommandPubTopic>
-      </plugin>
-    </gazebo>
-  </xacro:macro>
-
-  <!-- Macro to add the gps_plugin. -->
-  <xacro:macro name="gps_plugin_macro" params="namespace gps_noise">
-    <gazebo>
-      <plugin name="gps_plugin" filename="libgazebo_gps_plugin.so">
-        <robotNamespace>${namespace}</robotNamespace>
-        <gpsNoise>${gps_noise}</gpsNoise>
-      </plugin>
-    </gazebo>
-  </xacro:macro>
-
-  <!-- Macro to add the magnetometer_plugin. -->
-  <xacro:macro name="magnetometer_plugin_macro" params="namespace pub_rate noise_density random_walk bias_correlation_time mag_topic">
-    <gazebo>
-      <plugin name="magnetometer_plugin" filename="libgazebo_magnetometer_plugin.so">
-        <robotNamespace>${namespace}</robotNamespace>
-        <pubRate>${pub_rate}</pubRate>
-        <noiseDensity>${noise_density}</noiseDensity>
-        <randomWalk>${random_walk}</randomWalk>
-        <biasCorrelationTime>${bias_correlation_time}</biasCorrelationTime>
-        <magTopic>${mag_topic}</magTopic>
-      </plugin>
-    </gazebo>
-  </xacro:macro>
-
-  <!-- Macro to add the barometer_plugin. -->
-  <xacro:macro name="barometer_plugin_macro" params="namespace pub_rate baro_topic">
-    <gazebo>
-      <plugin name="barometer_plugin" filename="libgazebo_barometer_plugin.so">
-        <robotNamespace>${namespace}</robotNamespace>
-        <pubRate>${pub_rate}</pubRate>
-        <baroTopic>${baro_topic}</baroTopic>
-      </plugin>
-    </gazebo>
-  </xacro:macro>
-
-  <!-- Macro to add the mavlink interface. -->
-  <xacro:macro name="mavlink_interface_macro" params="namespace imu_sub_topic gps_sub_topic mag_sub_topic baro_sub_topic mavlink_addr mavlink_udp_port mavlink_tcp_port serial_enabled serial_device baudrate qgc_addr qgc_udp_port sdk_addr sdk_udp_port hil_mode hil_state_level vehicle_is_tailsitter send_vision_estimation send_odometry enable_lockstep use_tcp">
-    <gazebo>
-      <plugin name="mavlink_interface" filename="libgazebo_mavlink_interface.so">
-        <robotNamespace>${namespace}</robotNamespace>
-        <imuSubTopic>${imu_sub_topic}</imuSubTopic>
-        <gpsSubTopic>${gps_sub_topic}</gpsSubTopic>
-        <magSubTopic>${mag_sub_topic}</magSubTopic>
-        <baroSubTopic>${baro_sub_topic}</baroSubTopic>
-        <mavlink_addr>$(arg mavlink_addr)</mavlink_addr>
-        <mavlink_udp_port>$(arg mavlink_udp_port)</mavlink_udp_port>
-        <mavlink_tcp_port>$(arg mavlink_tcp_port)</mavlink_tcp_port>
-        <serialEnabled>$(arg serial_enabled)</serialEnabled>
-        <serialDevice>$(arg serial_device)</serialDevice>
-        <baudRate>$(arg baudrate)</baudRate>
-        <qgc_addr>$(arg qgc_addr)</qgc_addr>
-        <qgc_udp_port>$(arg qgc_udp_port)</qgc_udp_port>
-        <sdk_addr>$(arg sdk_addr)</sdk_addr>
-        <sdk_udp_port>$(arg sdk_udp_port)</sdk_udp_port>
-        <hil_mode>$(arg hil_mode)</hil_mode>
-        <hil_state_level>$(arg hil_state_level)</hil_state_level>
-        <vehicle_is_tailsitter>$(arg vehicle_is_tailsitter)</vehicle_is_tailsitter>
-        <send_vision_estimation>$(arg send_vision_estimation)</send_vision_estimation>
-        <send_odometry>$(arg send_odometry)</send_odometry>
-        <enable_lockstep>$(arg enable_lockstep)</enable_lockstep>
-        <use_tcp>$(arg use_tcp)</use_tcp>
-        <motorSpeedCommandPubTopic>/gazebo/command/motor_speed</motorSpeedCommandPubTopic>
-        <control_channels>
-          <channel name="rotor1">
-            <input_index>0</input_index>
-            <input_offset>0</input_offset>
-            <input_scaling>1000</input_scaling>
-            <zero_position_disarmed>0</zero_position_disarmed>
-            <zero_position_armed>100</zero_position_armed>
-            <joint_control_type>velocity</joint_control_type>
-          </channel>
-          <channel name="rotor2">
-            <input_index>1</input_index>
-            <input_offset>0</input_offset>
-            <input_scaling>1000</input_scaling>
-            <zero_position_disarmed>0</zero_position_disarmed>
-            <zero_position_armed>100</zero_position_armed>
-            <joint_control_type>velocity</joint_control_type>
-          </channel>
-          <channel name="rotor3">
-            <input_index>2</input_index>
-            <input_offset>0</input_offset>
-            <input_scaling>1000</input_scaling>
-            <zero_position_disarmed>0</zero_position_disarmed>
-            <zero_position_armed>100</zero_position_armed>
-            <joint_control_type>velocity</joint_control_type>
-          </channel>
-          <channel name="rotor4">
-            <input_index>3</input_index>
-            <input_offset>0</input_offset>
-            <input_scaling>1000</input_scaling>
-            <zero_position_disarmed>0</zero_position_disarmed>
-            <zero_position_armed>100</zero_position_armed>
-            <joint_control_type>velocity</joint_control_type>
-          </channel>
-          <channel name="rotor5">
-            <input_index>4</input_index>
-            <input_offset>1</input_offset>
-            <input_scaling>324.6</input_scaling>
-            <zero_position_disarmed>0</zero_position_disarmed>
-            <zero_position_armed>0</zero_position_armed>
-            <joint_control_type>velocity</joint_control_type>
-            <joint_control_pid>
-              <p>0.1</p>
-              <i>0</i>
-              <d>0</d>
-              <iMax>0.0</iMax>
-              <iMin>0.0</iMin>
-              <cmdMax>2</cmdMax>
-              <cmdMin>-2</cmdMin>
-            </joint_control_pid>
-            <joint_name>zephyr_delta_wing::propeller_joint</joint_name>
-          </channel>
-          <channel name="rotor6">
-            <input_index>5</input_index>
-            <input_offset>0</input_offset>
-            <!-- joint limits [-0.524, 0.524] -->
-            <input_scaling>0.524</input_scaling>
-            <zero_position_disarmed>0</zero_position_disarmed>
-            <zero_position_armed>0</zero_position_armed>
-            <joint_control_type>position</joint_control_type>
-            <joint_name>zephyr_delta_wing::flap_left_joint</joint_name>
-            <joint_control_pid>
-              <p>10.0</p>
-              <i>0</i>
-              <d>0</d>
-              <iMax>0</iMax>
-              <iMin>0</iMin>
-              <cmdMax>20</cmdMax>
-              <cmdMin>-20</cmdMin>
-            </joint_control_pid>
-          </channel>
-          <channel name="rotor7">
-            <input_index>6</input_index>
-            <input_offset>0</input_offset>
-            <!-- joint limits [-0.524, 0.524] -->
-            <input_scaling>0.524</input_scaling>
-            <zero_position_disarmed>0</zero_position_disarmed>
-            <zero_position_armed>0</zero_position_armed>
-            <joint_control_type>position</joint_control_type>
-            <joint_name>zephyr_delta_wing::flap_right_joint</joint_name>
-            <joint_control_pid>
-              <p>10.0</p>
-              <i>0</i>
-              <d>0</d>
-              <iMax>0</iMax>
-              <iMin>0</iMin>
-              <cmdMax>20</cmdMax>
-              <cmdMin>-20</cmdMin>
-            </joint_control_pid>
-          </channel>
-          <channel name="rotor8">
-            <input_index>7</input_index>
-            <input_offset>0</input_offset>
-            <input_scaling>0.524</input_scaling>
-            <zero_position_disarmed>0</zero_position_disarmed>
-            <zero_position_armed>0</zero_position_armed>
-            <joint_control_type>position</joint_control_type>
-          </channel>
-        </control_channels>
-      </plugin>
-    </gazebo>
-  </xacro:macro>
-
-  <!-- Macro to add an IMU. -->
-  <xacro:macro name="imu_plugin_macro"
-    params="namespace imu_suffix parent_link imu_topic
-      mass_imu_sensor gyroscope_noise_density gyroscopoe_random_walk
-      gyroscope_bias_correlation_time gyroscope_turn_on_bias_sigma
-      accelerometer_noise_density accelerometer_random_walk
-      accelerometer_bias_correlation_time accelerometer_turn_on_bias_sigma
-      *inertia *origin">
-    <!-- IMU link -->
-    <link name="${namespace}/imu${imu_suffix}_link">
-      <inertial>
-        <xacro:insert_block name="inertia" />
-        <mass value="${mass_imu_sensor}" />  <!-- [kg] -->
-        <origin xyz="0 0 0" rpy="0 0 0" />
-      </inertial>
-    </link>
-    <!-- IMU joint -->
-    <!-- For some reason, PX4 (and RotorS before them) used a "revolute" joint. Since IMU should be fixed relative to the drone frame, I'm changing this to "fixed", we'll see how that goes. -sfalexrog -->
-    <joint name="${namespace}/imu${imu_suffix}_joint" type="fixed">
-      <xacro:insert_block name="origin" />
-      <parent link="${parent_link}" />
-      <child link="${namespace}/imu${imu_suffix}_link" />
-      <!-- <limit upper="0" lower="0" effort="0" velocity="0" /> -->
-    </joint>
-    <!-- Well, now I know why: because Gazebo helpfully tries to "lump together" all fixed joints. Apparently there's a way to disable this behavior that is implemented here in its entirety: -->
-    <gazebo reference="${namespace}/imu${imu_suffix}_joint">
-      <disableFixedJointLumping>true</disableFixedJointLumping>
-      <preserveFixedJoint>true</preserveFixedJoint>
-    </gazebo>
-    <gazebo>
-      <plugin filename="libgazebo_imu_plugin.so" name="rotors_gazebo_imu${imu_suffix}_plugin">
-      <!-- A good description of the IMU parameters can be found in the kalibr documentation:
-           https://github.com/ethz-asl/kalibr/wiki/IMU-Noise-Model-and-Intrinsics -->
-        <robotNamespace>${namespace}</robotNamespace> <!-- (string, required): ros namespace in which the messages are published -->
-        <linkName>${namespace}/imu${imu_suffix}_link</linkName> <!-- (string, required): name of the body which holds the IMU sensor -->
-        <imuTopic>${imu_topic}</imuTopic> <!-- (string): name of the sensor output topic and prefix of service names (defaults to imu) -->
-        <gyroscopeNoiseDensity>${gyroscope_noise_density}</gyroscopeNoiseDensity> <!-- Gyroscope noise density (two-sided spectrum) [rad/s/sqrt(Hz)] -->
-        <gyroscopeRandomWalk>${gyroscopoe_random_walk}</gyroscopeRandomWalk> <!-- Gyroscope bias random walk [rad/s/s/sqrt(Hz)] -->
-        <gyroscopeBiasCorrelationTime>${gyroscope_bias_correlation_time}</gyroscopeBiasCorrelationTime> <!-- Gyroscope bias correlation time constant [s] -->
-        <gyroscopeTurnOnBiasSigma>${gyroscope_turn_on_bias_sigma}</gyroscopeTurnOnBiasSigma> <!-- Gyroscope turn on bias standard deviation [rad/s] -->
-        <accelerometerNoiseDensity>${accelerometer_noise_density}</accelerometerNoiseDensity> <!-- Accelerometer noise density (two-sided spectrum) [m/s^2/sqrt(Hz)] -->
-        <accelerometerRandomWalk>${accelerometer_random_walk}</accelerometerRandomWalk> <!-- Accelerometer bias random walk. [m/s^2/s/sqrt(Hz)] -->
-        <accelerometerBiasCorrelationTime>${accelerometer_bias_correlation_time}</accelerometerBiasCorrelationTime> <!-- Accelerometer bias correlation time constant [s] -->
-        <accelerometerTurnOnBiasSigma>${accelerometer_turn_on_bias_sigma}</accelerometerTurnOnBiasSigma> <!-- Accelerometer turn on bias standard deviation [m/s^2] -->
-      </plugin>
-    </gazebo>
-  </xacro:macro>
-
-<!-- Macro to add a generic odometry sensor. -->
-  <xacro:macro name="odometry_plugin_macro"
-    params="
-      namespace odometry_sensor_suffix parent_link pose_topic pose_with_covariance_topic
-      position_topic transform_topic odometry_topic parent_frame_id
-      mass_odometry_sensor measurement_divisor measurement_delay unknown_delay
-      noise_normal_position noise_normal_quaternion noise_normal_linear_velocity
-      noise_normal_angular_velocity noise_uniform_position
-      noise_uniform_quaternion noise_uniform_linear_velocity
-      noise_uniform_angular_velocity enable_odometry_map odometry_map
-      image_scale *inertia *origin">
-    <!-- odometry link -->
-    <link name="${namespace}/odometry_sensor${odometry_sensor_suffix}_link">
-      <inertial>
-        <xacro:insert_block name="inertia" />
-        <mass value="${mass_odometry_sensor}" />  <!-- [kg] -->
-      </inertial>
-    </link>
-    <!-- odometry joint -->
-    <joint name="${namespace}/odometry_sensor${odometry_sensor_suffix}_joint" type="revolute">
-      <parent link="${parent_link}" />
-      <xacro:insert_block name="origin" />
-      <child link="${namespace}/odometry_sensor${odometry_sensor_suffix}_link" />
-      <limit upper="0" lower="0" effort="0" velocity="0" />
-    </joint>
-    <gazebo>
-      <plugin filename="librotors_gazebo_odometry_plugin.so" name="odometry_sensor${odometry_sensor_suffix}">
-        <linkName>${namespace}/odometry_sensor${odometry_sensor_suffix}_link</linkName>
-        <robotNamespace>${namespace}</robotNamespace>
-        <poseTopic>${pose_topic}</poseTopic>
-        <poseWithCovarianceTopic>${pose_with_covariance_topic}</poseWithCovarianceTopic>
-        <positionTopic>${position_topic}</positionTopic>
-        <transformTopic>${transform_topic}</transformTopic>
-        <odometryTopic>${odometry_topic}</odometryTopic>
-        <parentFrameId>${parent_frame_id}</parentFrameId>  <!-- Use the scoped link name here. e.g. Model::link. -->
-        <measurementDivisor>${measurement_divisor}</measurementDivisor> <!-- only every (seq % measurementDivisor) == 0 measurement is published [int] -->
-        <measurementDelay>${measurement_delay}</measurementDelay> <!-- time that measurement gets held back before it's published in [simulation cycles (int)] -->
-        <unknownDelay>${unknown_delay}</unknownDelay> <!-- additional delay, that just gets added to the timestamp [s] -->
-        <noiseNormalPosition>${noise_normal_position}</noiseNormalPosition> <!-- standard deviation of additive white gaussian noise [m] -->
-        <noiseNormalQuaternion>${noise_normal_quaternion}</noiseNormalQuaternion> <!-- standard deviation white gaussian noise [rad]: q_m = q*quaternionFromSmallAngleApproximation(noiseNormalQ) -->
-        <noiseNormalLinearVelocity>${noise_normal_linear_velocity}</noiseNormalLinearVelocity> <!-- standard deviation of additive white gaussian noise [m/s] -->
-        <noiseNormalAngularVelocity>${noise_normal_angular_velocity}</noiseNormalAngularVelocity> <!-- standard deviation of additive white gaussian noise [rad/s] -->
-        <noiseUniformPosition>${noise_uniform_position}</noiseUniformPosition> <!-- symmetric bounds of uniform noise [m] -->
-        <noiseUniformQuaternion>${noise_uniform_quaternion}</noiseUniformQuaternion> <!-- symmetric bounds of uniform noise [rad], computation see above -->
-        <noiseUniformLinearVelocity>${noise_uniform_linear_velocity}</noiseUniformLinearVelocity>  <!-- symmetric bounds of uniform noise [m/s] -->
-        <noiseUniformAngularVelocity>${noise_uniform_angular_velocity}</noiseUniformAngularVelocity>  <!-- symmetric bounds of uniform noise [rad/s] -->
-        <xacro:if value="${enable_odometry_map}">
-          <covarianceImage>package://rotors_gazebo/resource/${odometry_map}</covarianceImage>
-          <covarianceImageScale>${image_scale}</covarianceImageScale>
-        </xacro:if>
-      </plugin>
-    </gazebo>
-  </xacro:macro>
-
-  <!-- Macro to add the wind plugin. -->
-  <xacro:macro name="wind_plugin_macro"
-    params="namespace xyz_offset wind_direction wind_force_mean
-      wind_gust_direction wind_gust_duration wind_gust_start
-      wind_gust_force_mean">
-    <gazebo>
-      <plugin filename="libgazebo_wind_plugin.so" name="wind_plugin">
-        <frameId>base_link</frameId>
-        <linkName>base_link</linkName>
-        <robotNamespace>${namespace}</robotNamespace>
-        <xyzOffset>${xyz_offset}</xyzOffset> <!-- [m] [m] [m] -->
-        <windDirection>${wind_direction}</windDirection>
-        <windForceMean>${wind_force_mean}</windForceMean> <!-- [N] -->
-        <windGustDirection>${wind_gust_direction}</windGustDirection>
-        <windGustDuration>${wind_gust_duration}</windGustDuration> <!-- [s] -->
-        <windGustStart>${wind_gust_start}</windGustStart> <!-- [s] -->
-        <windGustForceMean>${wind_gust_force_mean}</windGustForceMean> <!-- [N] -->
-      </plugin>
-    </gazebo>
-  </xacro:macro>
-
-  <!-- VI sensor macros -->
-  <!-- Macro to add a VI-sensor camera. -->
-  <xacro:macro name="vi_sensor_camera_macro"
-    params="namespace parent_link camera_suffix frame_rate *origin">
-    <xacro:camera_macro
-      namespace="${namespace}"
-      parent_link="${parent_link}"
-      camera_suffix="${camera_suffix}"
-      frame_rate="${frame_rate}"
-      horizontal_fov="1.3962634"
-      image_width="752"
-      image_height="480"
-      image_format="L8"
-      min_distance="0.02"
-      max_distance="30"
-      noise_mean="0.0"
-      noise_stddev="0.007"
-      enable_visual="false">
-      <cylinder length="0.01" radius="0.007" />
-      <xacro:insert_block name="origin" />
-    </xacro:camera_macro>
-  </xacro:macro>
-
-  <!-- Macro to add a depth camera on the VI-sensor. -->
-  <xacro:macro name="vi_sensor_depth_macro"
-    params="namespace parent_link camera_suffix frame_rate *origin">
-    <link name="${namespace}/camera_${camera_suffix}_link">
-      <collision>
-        <origin xyz="0 0 0" rpy="0 0 0" />
-        <geometry>
-          <cylinder length="0.01" radius="0.007" />
-        </geometry>
-      </collision>
-      <inertial>
-        <mass value="1e-5" />
-        <origin xyz="0 0 0" rpy="0 0 0" />
-        <inertia ixx="1e-6" ixy="0" ixz="0" iyy="1e-6" iyz="0" izz="1e-6" />
-      </inertial>
-    </link>
-    <joint name="${namespace}/camera_${camera_suffix}_joint" type="fixed">
-      <xacro:insert_block name="origin" />
-      <parent link="${parent_link}" />
-      <child link="${namespace}/camera_${camera_suffix}_link" />
-      <limit upper="0" lower="0" effort="0" velocity="0" />
-    </joint>
-    <!-- Optical center of camera -->
-    <link name="${namespace}/camera_${camera_suffix}_optical_center_link" />
-    <joint name="${namespace}/camera_${camera_suffix}_optical_center_joint" type="fixed">
-      <origin xyz="0 0 0" rpy="${-PI/2} 0 ${-PI/2}" />
-      <parent link="${namespace}/camera_${camera_suffix}_link" />
-      <child link="${namespace}/camera_${camera_suffix}_optical_center_link" />
-      <limit upper="0" lower="0" effort="0" velocity="0" />
-    </joint>
-    <gazebo reference="${namespace}/camera_${camera_suffix}_link">
-      <sensor type="depth" name="${namespace}_camera_{camera_suffix}">
-        <always_on>true</always_on>
-        <update_rate>${frame_rate}</update_rate>
-        <camera>
-          <horizontal_fov>2</horizontal_fov>
-          <image>
-            <format>L8</format>
-            <width>640</width>
-            <height>480</height>
-          </image>
-          <clip>
-            <near>0.01</near>
-            <far>100</far>
-          </clip>
-        </camera>
-        <plugin name="${namespace}_camera_{camera_suffix}" filename="libgazebo_ros_openni_kinect.so">
-          <robotNamespace>${namespace}</robotNamespace>
-          <alwaysOn>true</alwaysOn>
-          <baseline>0.11</baseline>
-          <updateRate>${frame_rate}</updateRate>
-          <cameraName>camera_${camera_suffix}</cameraName>
-          <imageTopicName>camera/image_raw</imageTopicName>
-          <cameraInfoTopicName>camera/camera_info</cameraInfoTopicName>
-          <depthImageTopicName>depth/disparity</depthImageTopicName>
-          <depthImageCameraInfoTopicName>depth/camera_info</depthImageCameraInfoTopicName>
-          <pointCloudTopicName>depth/points</pointCloudTopicName>
-          <frameName>camera_${camera_suffix}_optical_center_link</frameName>
-          <pointCloudCutoff>0.5</pointCloudCutoff>
-          <distortionK1>0.0</distortionK1>
-          <distortionK2>0.0</distortionK2>
-          <distortionK3>0.0</distortionK3>
-          <distortionT1>0.0</distortionT1>
-          <distortionT2>0.0</distortionT2>
-        </plugin>
-      </sensor>
-    </gazebo>
-  </xacro:macro>
-
-  <!-- VI-Sensor Macro -->
-  <xacro:macro name="vi_sensor_macro" params="namespace parent_link *origin">
-    <!-- Vi Sensor Link -->
-    <link name="${namespace}/vi_sensor_link">
-      <collision>
-        <origin xyz="0 0 0" rpy="0 0 0" />
-        <geometry>
-          <box size="0.03 0.133 0.057" />
-        </geometry>
-      </collision>
-
-      <visual>
-        <origin xyz="0 0 0" rpy="0 0 0" />
-        <geometry>
-          <mesh filename="package://rotors_description/meshes/vi_sensor.dae" scale="1 1 1" />
-        </geometry>
-      </visual>
-
-      <inertial>
-        <mass value="0.13" />
-        <origin xyz="0 0 0" rpy="0 0 0" />
-        <inertia ixx="1e-6" ixy="0" ixz="0" iyy="1e-6" iyz="0" izz="1e-6" />
-      </inertial>
-    </link>
-    <joint name="{namespace}_vi_sensor_joint" type="fixed">
-      <xacro:insert_block name="origin" />
-      <parent link="${parent_link}" />
-      <child link="${namespace}/vi_sensor_link" />
-    </joint>
-    <!-- Cameras -->
-    <xacro:if value="$(arg enable_cameras)">
-      <!-- Left Camera -->
-      <xacro:vi_sensor_camera_macro
-        namespace="${namespace}" parent_link="${namespace}/vi_sensor_link"
-        camera_suffix="left" frame_rate="30.0">
-        <origin xyz="0.015 0.055 0.0065" rpy="0 0 0" />
-      </xacro:vi_sensor_camera_macro>
-      <!-- Right Camera -->
-      <xacro:vi_sensor_camera_macro namespace="${namespace}"
-        parent_link="${namespace}/vi_sensor_link"
-        camera_suffix="right" frame_rate="30.0">
-        <origin xyz="0.015 -0.055 0.0065" rpy="0 0 0" />
-      </xacro:vi_sensor_camera_macro>
-    </xacro:if>
-
-    <!-- Depth Sensor -->
-    <xacro:if value="$(arg enable_depth)">
-      <xacro:vi_sensor_depth_macro
-        namespace="${namespace}" parent_link="${namespace}/vi_sensor_link"
-        camera_suffix="depth" frame_rate="30.0">
-        <origin xyz="0.015 0.055 0.0065" rpy="0 0 0" />
-      </xacro:vi_sensor_depth_macro>
-    </xacro:if>
-
-    <!-- Groundtruth -->
-    <xacro:if value="$(arg enable_ground_truth)">
-      <!-- Odometry Sensor -->
-      <xacro:odometry_plugin_macro
-        namespace="${namespace}/ground_truth"
-        odometry_sensor_suffix=""
-        parent_link="${namespace}/vi_sensor_link"
-        pose_topic="pose"
-        pose_with_covariance_topic="pose_with_covariance"
-        position_topic="position"
-        transform_topic="transform"
-        odometry_topic="odometry"
-        parent_frame_id="world"
-        mass_odometry_sensor="0.00001"
-        measurement_divisor="1"
-        measurement_delay="0"
-        unknown_delay="0.0"
-        noise_normal_position="0 0 0"
-        noise_normal_quaternion="0 0 0"
-        noise_normal_linear_velocity="0 0 0"
-        noise_normal_angular_velocity="0 0 0"
-        noise_uniform_position="0 0 0"
-        noise_uniform_quaternion="0 0 0"
-        noise_uniform_linear_velocity="0 0 0"
-        noise_uniform_angular_velocity="0 0 0"
-        enable_odometry_map="false"
-        odometry_map=""
-        image_scale="">
-        <inertia ixx="0.00001" ixy="0.0" ixz="0.0" iyy="0.00001" iyz="0.0" izz="0.00001" /> <!-- [kg.m^2] [kg.m^2] [kg.m^2] [kg.m^2] [kg.m^2] [kg.m^2] -->
-        <origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0" />
-      </xacro:odometry_plugin_macro>
-    </xacro:if>
-
-    <!-- ADIS16448 IMU. -->
-    <xacro:imu_plugin_macro
-      namespace="${namespace}"
-      imu_suffix=""
-      parent_link="${namespace}/vi_sensor_link"
-      imu_topic="/imu"
-      mass_imu_sensor="0.015"
-      gyroscope_noise_density="0.0003394"
-      gyroscopoe_random_walk="0.000038785"
-      gyroscope_bias_correlation_time="1000.0"
-      gyroscope_turn_on_bias_sigma="0.0087"
-      accelerometer_noise_density="0.004"
-      accelerometer_random_walk="0.006"
-      accelerometer_bias_correlation_time="300.0"
-      accelerometer_turn_on_bias_sigma="0.1960">
-      <inertia ixx="0.00001" ixy="0.0" ixz="0.0" iyy="0.00001" iyz="0.0" izz="0.00001" />
-      <origin xyz="0.015 0 0.0113" rpy="0 0 0" />
-    </xacro:imu_plugin_macro>
-  </xacro:macro>
-
-</robot>

clover_description/urdf/leds/led_strip.xacro

@@ -1,69 +0,0 @@
-<?xml version="1.0" encoding="UTF-8"?>
-<robot name="led_strip" xmlns:xacro="http://ros.org/wiki/xacro">
-
-    <!-- LED strip macro
-        Parameters:
-          name: Name of the LED strip
-          parent: Name of the parent link (likely to be base_link)
-          radius: Radius of the strip
-          bulb_radius: Radius (for spheres)/side (for boxes) of the LED
-          led_count: Number of LEDs in a strip
-          use_plugin: Attach the controller plugin to LEDs
-    -->
-    <xacro:macro name="led_strip" params="name parent radius:=0.08 bulb_radius:=0.005 led_count:=58 shift:=0 use_plugin:=false x:=0 y:=0 z:=0 roll:=0 pitch:=0 yaw:=0">
-        <joint name="${name}_joint" type="fixed">
-            <origin xyz="${x} ${y} ${z}"
-                    rpy="${roll} ${pitch} ${yaw}"/>
-            <parent link="${parent}"/>
-            <child link="${name}_link"/>
-        </joint>
-
-        <link name="${name}_link">
-            <inertial>
-                <origin xyz="0 0 0"/>
-                <mass value="0.0001"/>
-                <inertia ixx="0.001" ixy="0.0" ixz="0.0" iyy="0.001" iyz="0.0" izz="0.001"/>
-            </inertial>
-
-            <xacro:macro name="loop_links" params="link_i links_total">
-                <visual>
-                    <origin xyz="${radius*cos(2 * pi * link_i / links_total)} ${radius*sin(2 * pi * link_i / links_total)} 0"
-                            rpy="0 0 ${2 * pi * link_i / links_total}"/>
-                    <geometry>
-                        <box size="${bulb_radius} ${bulb_radius} ${bulb_radius}"/>
-                    </geometry>
-                </visual>
-                <xacro:unless value="${link_i + 1 == links_total}">
-                    <xacro:loop_links link_i="${link_i + 1}" links_total="${links_total}"/>
-                </xacro:unless>
-            </xacro:macro>
-            <xacro:loop_links link_i="0" links_total="${led_count}"/>
-        </link>
-
-        <xacro:if value="${use_plugin}">
-        <gazebo reference="${name}_link">
-            <visual>
-                <plugin name="${name}_controller" filename="libsim_leds.so">
-                    <robotNamespace></robotNamespace>
-                    <ledCount>${led_count}</ledCount>
-                </plugin>
-                <cast_shadows>false</cast_shadows>
-                <material>
-                    <ambient>0 0 0 1</ambient>
-                    <diffuse>0 0 0 1</diffuse>
-                    <specular>0 0 0 1</specular>
-                    <emissive>0 0 0 1</emissive>
-                </material>
-            </visual>
-        </gazebo>
-        </xacro:if>
-        <gazebo reference="${name}_joint">
-            <disableFixedJointLumping>true</disableFixedJointLumping>
-            <preserveFixedJoint>true</preserveFixedJoint>
-        </gazebo>
-        <!-- <gazebo>
-            <static>true</static>
-        </gazebo> -->
-    </xacro:macro>
-
-</robot>

clover_description/urdf/sensors/distance_sensor.urdf.xacro

@@ -1,73 +0,0 @@
-<?xml version="1.0" encoding="UTF-8"?>
-<robot name="rpi_camera" xmlns:xacro="http://ros.org/wiki/xacro">
-  <xacro:include filename="$(find clover_description)/urdf/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">
-    <joint name="${name}_joint" type="fixed">
-      <origin xyz="${x} ${y} ${z}"
-              rpy="${roll} ${pitch} ${yaw}"/>
-      <parent link="${parent}"/>
-      <child link="${name}_link"/>
-    </joint>
-
-    <link name="${name}_link">
-      <visual>
-        <origin xyz="0 0 0" rpy="0 0 0"/>
-        <geometry>
-          <box size="0.001 0.005 0.005"/>
-        </geometry>
-      </visual>
-      <collision>
-        <origin xyz="0 0 0" rpy="0 0 0"/>
-        <geometry>
-          <box size="0.001 0.001 0.005"/>
-        </geometry>
-      </collision>
-      <inertial>
-        <mass value="0.015"/>
-        <origin xyz="0 0 0"/>
-        <inertia ixx="4.15e-6" iyy="2.407e-6" izz="2.407e-6" ixy="0" ixz="0" iyz="0"/>
-      </inertial>
-    </link>
-
-    <gazebo reference="${name}_joint">
-      <disableFixedJointLumping>true</disableFixedJointLumping>
-      <preserveFixedJoint>true</preserveFixedJoint>
-    </gazebo>
-
-    <gazebo reference="${name}_link">
-      <sensor type="ray" name="${name}">
-        <pose>0 0 0 0 0 0</pose>
-        <ray>
-          <scan>
-            <horizontal>
-              <samples>1</samples>
-              <resolution>1</resolution>
-              <min_angle>-0</min_angle>
-              <max_angle>0</max_angle>
-            </horizontal>
-          </scan>
-          <range>
-            <min>0.03</min> <!-- Note: This is a hack, since the rangefinder is inside the drone collision box -->
-            <max>${range_max}</max>
-            <resolution>${resolution}</resolution>
-          </range>
-        </ray>
-        <plugin name="laser" filename="libgazebo_ros_range.so">
-          <robotNamespace></robotNamespace> <!-- FIXME: fill namespace? -->
-          <topicName>/rangefinder/range</topicName>
-          <frameName>rangefinder</frameName>
-          <radiation>infrared</radiation>
-          <fov>0.01</fov>
-          <gaussianNoise>0.001</gaussianNoise>
-          <updateRate>${rate}</updateRate>
-          <min_distance>${range_min}</min_distance>
-          <max_distance>${range_max}</max_distance>
-        </plugin>
-        <always_on>1</always_on>
-        <update_rate>${rate}</update_rate>
-        <visualize>true</visualize>
-      </sensor>
-    </gazebo>
-  </xacro:macro>
-</robot>

clover_description/urdf/sensors/pinhole_camera.urdf.xacro

@@ -1,5 +0,0 @@
-<?xml version="1.0" encoding="UTF-8"?>
-<robot name="pinhole_camera" xmlns:xacro="http://ros.org/wiki/xacro">
-  <xacro:include filename="$(find clover_description)/urdf/camera_sensor.urdf.xacro"/>
-  <xacro:camera_sensor name="main_camera" width="640" height="480" rate="30" horizontal_fov="${120.0*pi/180.0}"/>
-</robot>

clover_description/urdf/sensors/rpi_cam.urdf.xacro

@@ -1,60 +0,0 @@
-<?xml version="1.0" encoding="UTF-8"?>
-<robot name="rpi_camera" xmlns:xacro="http://ros.org/wiki/xacro">
-  <xacro:include filename="$(find clover_description)/urdf/camera_sensor.urdf.xacro"/>
-
-  <xacro:macro name="rpi_cam" params="name:=rpi_cam parent x:=0 y:=0 z:=0 roll:=0 pitch:=0 yaw:=0 width:=320 height:=240 rate:=40 do_throttling:=false">
-    <joint name="${name}_joint" type="fixed">
-      <origin xyz="${x} ${y} ${z}"
-              rpy="${roll} ${pitch} ${yaw}"/>
-      <parent link="${parent}"/>
-      <child link="${name}_link"/>
-    </joint>
-
-    <link name="${name}_link">
-      <visual>
-        <origin xyz="0 0 0" rpy="0 0 0"/>
-        <geometry>
-          <box size="0.005 0.005 0.005"/>
-        </geometry>
-      </visual>
-      <collision>
-        <origin xyz="0 0 0" rpy="0 0 0"/>
-        <geometry>
-          <box size="0.005 0.005 0.005"/>
-        </geometry>
-      </collision>
-      <inertial>
-        <mass value="0.015"/>
-        <origin xyz="0 0 0"/>
-        <inertia ixx="4.15e-6" iyy="2.407e-6" izz="2.407e-6" ixy="0" ixz="0" iyz="0"/>
-      </inertial>
-    </link>
-    <gazebo reference="${name}_joint">
-      <disableFixedJointLumping>true</disableFixedJointLumping>
-      <preserveFixedJoint>true</preserveFixedJoint>
-    </gazebo>
-    <!-- <xacro:camera_sensor_base name="${name}"
-      frame_name="/${name}_optical"
-      width="${width}"
-      height="${height}"
-      rate="${rate}"
-      horizontal_fov="1.6075201319067056"
-      k1="-0.273377"
-      k2="0.0642871"
-      p1="-0.00086158"
-      p2="-0.000443529"
-      k3="-0.00599387"
-      cx="158.0735"
-      cy="108.513"
-      cx_prime="158.0735"
-      /> -->
-    <xacro:camera_sensor name="${name}"
-      width="${width}"
-      height="${height}"
-      rate="${rate}"
-      horizontal_fov="${120 * pi / 180}"
-      do_throttling="${do_throttling}"
-    />
-  </xacro:macro>
-
-</robot>

clover_simulation/CMakeLists.txt

@@ -1,60 +0,0 @@
-cmake_minimum_required(VERSION 3.0)
-project(clover_simulation)
-
-find_package(catkin REQUIRED)
-
-# Gazebo LED strip node
-
-find_package(gazebo)
-
-# Gazebo does not seem to export GAZEBO_FOUND, so here's a matching hack
-if (NOT "${GAZEBO_CONFIG_INCLUDED}")
-  message(STATUS "Gazebo not found, skipping")
-  return()
-endif()
-
-find_package(catkin REQUIRED COMPONENTS
-  roscpp
-  led_msgs
-  gazebo_ros
-  gazebo_plugins
-  rospy
-)
-
-catkin_python_setup()
-
-catkin_package(
-  CATKIN_DEPENDS led_msgs gazebo_ros gazebo_plugins
-)
-
-# Gazebo LED plugin
-
-add_library(sim_leds src/sim_leds.cpp)
-
-target_include_directories(sim_leds PRIVATE ${catkin_INCLUDE_DIRS} ${GAZEBO_INCLUDE_DIRS})
-target_link_libraries(sim_leds PRIVATE ${catkin_LIBRARIES} ${GAZEBO_LIBRARIES})
-target_compile_options(sim_leds PRIVATE -std=c++11)
-
-add_dependencies(sim_leds ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
-
-# Gazebo throttling camera plugin
-# for some reason, CMake does not support per-target link directories, and Gazebo does not put
-# CameraPlugin into ${GAZEBO_LIBRARIES}
-
-link_directories(${GAZEBO_LIBRARY_DIRS})
-
-add_library(throttling_camera src/throttling_camera.cpp)
-
-target_include_directories(throttling_camera PRIVATE ${catkin_INCLUDE_DIRS} ${GAZEBO_INCLUDE_DIRS})
-target_link_libraries(throttling_camera PRIVATE ${catkin_LIBRARIES} ${GAZEBO_LIBRARIES} CameraPlugin)
-target_compile_options(throttling_camera PRIVATE -std=c++11)
-
-add_dependencies(throttling_camera ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
-
-install(DIRECTORY launch DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION})
-install(DIRECTORY meshes DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION})
-install(DIRECTORY resources DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION})
-
-catkin_install_python(PROGRAMS scripts/aruco_gen
-  DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
-)

clover_simulation/README.md

@@ -1,82 +0,0 @@
-# `clover_simulation` ROS package
-
-This package provides resources necessary for launching Gazebo simulation with Clover, along with `.launch` files for convenience.
-
-## Launching the simulation
-
-Simulation is launched by [`simulator.launch` file](launch/simulator.launch). This `.launch` file assumes that `px4` and `sitl_gazebo` packages are reachable from your current workspace.
-
-The simulation may be configured by a set of arguments:
-
-* `mav_id` (*integer*, default: *0*) - MAVLink identifier of the vehicle. **Note**: Multi-vehicle simulation is possible, but requires extensive changes to launch files;
-* `est` (*string*, default: *lpe*, possible values: *lpe*, *ekf2*) - PX4 estimator selection. Note that this may be overriden in the startup scripts for your craft;
-* `vehicle` (*string*, default: *clover*) - PX4 vehicle name. Depending on this parameter, different PX4 presets will be loaded. **Note**: The default value, *clover*, requires you to use [Clover-specific PX4 branch](https://github.com/CopterExpress/Firmware/tree/v1.10.1-clever);
-* `main_camera` (*boolean*, default: *true*) - controls whether the drone will have a vision position estimation camera;
-* `rangefinder` (*boolean*, default: *true*) - controls whether the drone will have a laser rangefinder;
-* `led` (*boolean*, default: *true*) - controls whether the drone will have a programmable LED strip;
-* `gps` (*boolean*, default: *true*) - controls whether the drone will have a simulated GPS module;
-
-In order to start the simulation, run:
-
-```bash
-roslaunch clover_simulation simulator.launch
-```
-
-This will start a new Gazebo instance (using `gazebo_ros` package), load a PX4 SITL instance, spawn a Clover model and start Clover ROS nodes. The PX4 console will be accessible in the terminal where `roslaunch` was performed.
-
-### Changing simulation speed (PX4 1.9+)
-
-In order to run simulation faster or slower than realtime, use the `PX4_SIM_SPEED_FACTOR` environment variable, [as stated in the PX4 docs](https://dev.px4.io/v1.9.0/en/simulation/#simulation_speed).
-
-If `PX4_SIM_SPEED_FACTOR` is not set, it is assumed that it is equal to 1.0.
-
-Note that Gazebo may slow the simulation down automatically. This may not be handled gracefully, so if you notice Gazebo's "Real Time Factor" being significantly lower than your `PX4_SIM_SPEED_FACTOR`, be sure to adjust it accordingly.
-
-### Changing initial world
-
-By default, the `simulator.launch` file will start the simulation with [`resources/worlds/clover.world`](resources/worlds/clover.world) as its base world. Note that the `real_time_update_rate` is set to 250 - this is required for PX4 lockstep simulation to work correctly.
-
-If you wish to create your own world for the simulation, be sure to derive it from `clover.world` to avoid issues with PX4 plugins.
-
-You may set the world name in `simulator.launch` as the `world_name` parameter for `gazebo_ros` instance.
-
-### Configuring the vehicle
-
-`simulator.launch` utilizes the same `clover.launch` file from the `clover` ROS package, so ROS node reconfiguration is the same as on the real drone.
-
-PX4 may be reconfigured using QGroundControl, just like a real drone. Some parameters may require rebooting the drone, which is performed by shutting the simulated environment down and restarting it.
-
-PX4 will write its parameters and logs to `${ROS_HOME}/eeprom/parameters` and `${ROS_HOME}/log`, respectively. Note that the log directory naming schema for PX4 logs is different from ROS: PX4 creates log directories based on the current date, which makes them relatively simple to find.
-
-## LED plugin (sim_leds)
-
-A visual Gazebo plugin is used for the LED strip. An example of the plugin usage is provided in [`led_strip.xacro`](../clover_description/urdf/leds/led_strip.xacro).
-
-The plugin accepts the following parameters during instantiation:
-
-* `robotNamespace` (*string*, default: "") - a ROS namespace for the plugin;
-* `ledCount` (*integer*, required) - total numer of LEDs in a strip.
-
-The plugin will provide the following service:
-
-`led/set_leds` ([*led_msgs/SetLEDs*](https://github.com/CopterExpress/ros_led/blob/v0.0.6/led_msgs/srv/SetLEDs.srv)) - set the LED colors to the provided values.
-
-The plugin will provide the following topics:
-
-`led/state` ([*led_msgs/LEDStateArray*](https://github.com/CopterExpress/ros_led/blob/v0.0.6/led_msgs/msg/LEDStateArray.msg)) - current LED strip state.
-
-Other nodes are not expected to write to `led/state` topic.
-
-All provided topics and services will be namespaced according to the `robotNamespace` parameter.
-
-## Throttling camera plugin (throttling_camera)
-
-By default, Gazebo camera sensors will use their `update_rate` parameter as an upper bound for the actual rate. This may result in much lower rates than expected. This may be fine for object recognition tasks where the camera is not the primary positioning sensor, but is not desirable in our case, when the camera is used for position calculation.
-
-We provide a Gazebo-ROS plugin for the camera sensor that will throttle down the simulation to maintain update rate. The plugin API is based on the `gazebo_ros_camera` plugin, and respects the following parameters in SDF:
-
-* `<minUpdateRate>` (*double*, default: same as `<updateRate>`) - least allowed publish/update rate for the camera (in Hz);
-* `<windowSize>` (*integer*, default: 10) - number of last update intervals that are considered for throttling;
-* `<maxStDev>` (*double*, default: 0.02) - maximum standard deviation value for update intervals.
-
-The simulation will be slowed down if the average update rate (averaged over `<windowSize>` samples) is lower than `<minUpdateRate>` and is consistent (standard deviation over `<windowSize>` samples is less than `<maxStDev>`).

clover_simulation/launch/simulator.launch

@@ -1,41 +0,0 @@
-<launch>
-    <arg name="mav_id" default="0"/>
-    <arg name="est" default="ekf2"/> <!-- PX4 estimator: lpe, ekf2 -->
-    <arg name="vehicle" default="clover"/> <!-- PX4 vehicle configuration: clover, clover_vpe -->
-    <arg name="main_camera" default="true"/> <!-- Simulated vision position estimation camera (optical flow, ArUco) -->
-    <arg name="rangefinder" default="true"/> <!-- Simulated downward-facing rangefinder, vl53l1x-like -->
-    <arg name="led" default="true"/> <!-- Simulated LED strip, ws281x-like -->
-    <arg name="gps" default="false"/> <!--Simulated GPS module -->
-    <arg name="use_clover_physics" default="false"/> <!-- Use inertial parameters from CAD models -->
-
-    <!-- Gazebo instance -->
-    <include file="$(find gazebo_ros)/launch/empty_world.launch">
-        <!-- Workaround for crashes in VMware -->
-        <env name="SVGA_VGPU10" value="0"/>
-        <arg name="gui" value="true"/>
-        <arg name="world_name" value="$(find clover_simulation)/resources/worlds/clover.world"/>
-        <arg name="verbose" value="true"/>
-    </include>
-
-    <!-- PX4 instance -->
-    <node name="sitl_$(arg mav_id)" pkg="px4" type="px4" output="screen" args="$(find px4)/ROMFS/px4fmu_common -s etc/init.d-posix/rcS -i $(arg mav_id)">
-        <env name="PX4_SIM_MODEL" value="$(arg vehicle)"/>
-        <env name="PX4_ESTIMATOR" value="$(arg est)"/>
-    </node>
-
-    <!-- Clover model -->
-    <include file="$(find clover_description)/launch/spawn_drone.launch">
-        <arg name="main_camera" value="$(arg main_camera)"/>
-        <arg name="rangefinder" value="$(arg rangefinder)"/>
-        <arg name="led" value="$(arg led)"/>
-        <arg name="gps" value="$(arg gps)"/>
-        <arg name="use_clover_physics" value="$(arg use_clover_physics)"/>
-    </include>
-
-    <!-- Clover services -->
-    <include file="$(find clover)/launch/clover.launch">
-        <arg name="simulator" value="true"/>
-        <arg name="fcu_conn" value="sitl"/>
-        <arg name="fcu_ip" value="127.0.0.1"/>
-    </include>
-</launch>

clover_simulation/launch/simulator_1.8.2.launch

@@ -1,36 +0,0 @@
-<launch>
-    <arg name="mav_id" default="0"/>
-    <arg name="est" default="lpe"/> <!-- PX4 estimator: lpe, ekf2 -->
-    <arg name="vehicle" default="iris"/>
-    <arg name="main_camera" default="true"/> <!-- Simulated vision position estimation camera (optical flow, ArUco) -->
-    <arg name="rangefinder" default="true"/> <!-- Simulated downward-facing rangefinder, vl53l1x-like -->
-    <arg name="led" default="true"/> <!-- Simulated LED strip, ws281x-like -->
-    <arg name="gps" default="false"/> <!--Simulated GPS module -->
-
-    <!-- Gazebo instance -->
-    <include file="$(find gazebo_ros)/launch/empty_world.launch">
-        <!-- Workaround for crashes in VMware -->
-        <env name="SVGA_VGPU10" value="0"/>
-        <arg name="gui" value="true"/>
-        <arg name="world_name" value="$(find clover_simulation)/resources/worlds/clover.world"/>
-        <arg name="verbose" value="true"/>
-    </include>
-
-    <!-- PX4 instance -->
-    <node name="sitl" pkg="px4" type="px4" output="screen" args="$(find px4) $(find px4)/posix-configs/SITL/init/$(arg est)/$(arg vehicle)" required="true"/>
-
-    <!-- Clover model -->
-    <include file="$(find clover_description)/launch/spawn_drone.launch">
-        <arg name="main_camera" value="$(arg main_camera)"/>
-        <arg name="rangefinder" value="$(arg rangefinder)"/>
-        <arg name="led" value="$(arg led)"/>
-        <arg name="gps" value="$(arg gps)"/>
-    </include>
-
-    <!-- Clover services -->
-    <include file="$(find clover)/launch/clover.launch">
-        <arg name="simulator" value="true"/>
-        <arg name="fcu_conn" value="udp"/>
-        <arg name="fcu_ip" value="127.0.0.1"/>
-    </include>
-</launch>

clover_simulation/models/loop_line/loop_line.sdf

@@ -1,24 +0,0 @@
-<?xml version="1.0"?>
-<sdf version="1.5">
-    <model name="loop_line">
-        <static>true</static>
-        <link name="loop_link">
-            <pose>0 0 1e-3 0 0 0</pose>
-            <visual name="loop_texture">
-                <cast_shadows>false</cast_shadows>
-                <geometry>
-                    <box>
-                        <size>8.0 3.2 1e-3</size>
-                    </box>
-                </geometry>
-                <material>
-                    <script>
-                        <uri>model://loop_line/materials/scripts</uri>
-                        <uri>model://loop_line/materials/textures</uri>
-                        <name>loop_dashed</name>
-                    </script>
-                </material>
-            </visual>
-        </link>
-    </model>
-</sdf>

clover_simulation/models/loop_line/materials/scripts/loop.material

@@ -1,33 +0,0 @@
-material loop_dashed
-{
-	technique
-	{
-		pass
-		{
-			scene_blend alpha_blend
-			texture_unit
-			{
-				texture loop_dashed.png
-				filtering none
-				scale 1.0 1.0
-			}
-		}
-	}
-}
-
-material loop_solid
-{
-	technique
-	{
-		pass
-		{
-			scene_blend alpha_blend
-			texture_unit
-			{
-				texture loop_solid.png
-				filtering none
-				scale 1.0 1.0
-			}
-		}
-	}
-}

clover_simulation/models/loop_line/model.config

@@ -1,13 +0,0 @@
-<?xml version="1.0"?>
-<model>
-    <name>Loop Line</name>
-    <version>1.0</version>
-    <sdf version="1.5">loop_line.sdf</sdf>
-    <author>
-        <name>Oleg Kalachev</name>
-        <email>okalachev@gmail.com</email>
-    </author>
-    <description>
-        Black loop line on the floor for recognizing by a drone
-    </description>
-</model>

clover_simulation/models/parquet_plane/materials/scripts/parquet.material

@@ -1,20 +0,0 @@
-material parquet
-{
-	technique
-	{
-		pass
-		{
-			ambient 0.5 0.5 0.5 1.0
-			diffuse 0.5 0.5 0.5 1.0
-			specular 0.2 0.2 0.2 1.0 12.5
-
-			texture_unit
-			{
-				texture parquet.jpg
-				filtering anistropic
-				max_anisotropy 16
-				scale 0.01 0.01
-			}
-		}
-	}
-}

clover_simulation/models/parquet_plane/model.config

@@ -1,17 +0,0 @@
-<?xml version="1.0"?>
-
-<model>
-  <name>Parquet Plane</name>
-  <version>1.0</version>
-  <sdf version="1.5">model.sdf</sdf>
-
-  <author>
-    <name>Oleg Kalachev</name>
-    <email>okalachev@gmail.com</email>
-  </author>
-
-  <description>
-    A parquet textured plane.
-  </description>
-
-</model>

clover_simulation/models/parquet_plane/model.sdf

@@ -1,30 +0,0 @@
-<?xml version="1.0" ?>
-<sdf version="1.5">
-  <model name="parquet_plane">
-    <static>true</static>
-    <link name="link">
-      <collision name="collision">
-        <geometry>
-          <box>
-            <size>200 200 .02</size>
-          </box>
-        </geometry>
-      </collision>
-      <visual name="visual">
-        <cast_shadows>false</cast_shadows>
-        <geometry>
-          <box>
-            <size>200 200 .02</size>
-          </box>
-        </geometry>
-        <material>
-          <script>
-            <uri>model://parquet_plane/materials/scripts</uri>
-            <uri>model://parquet_plane/materials/textures</uri>
-            <name>parquet</name>
-          </script>
-        </material>
-      </visual>
-    </link>
-  </model>
-</sdf>

clover_simulation/models/square_line/materials/scripts/square.material

@@ -1,33 +0,0 @@
-material square_dashed
-{
-	technique
-	{
-		pass
-		{
-			scene_blend alpha_blend
-			texture_unit
-			{
-				texture square_dashed.png
-				filtering none
-				scale 1.0 1.0
-			}
-		}
-	}
-}
-
-material square_solid
-{
-	technique
-	{
-		pass
-		{
-			scene_blend alpha_blend
-			texture_unit
-			{
-				texture square_solid.png
-				filtering none
-				scale 1.0 1.0
-			}
-		}
-	}
-}

clover_simulation/models/square_line/model.config

@@ -1,13 +0,0 @@
-<?xml version="1.0"?>
-<model>
-    <name>Square Line</name>
-    <version>1.0</version>
-    <sdf version="1.5">square_line.sdf</sdf>
-    <author>
-        <name>Oleg Kalachev</name>
-        <email>okalachev@gmail.com</email>
-    </author>
-    <description>
-        Black square line on the floor for recognizing by a drone
-    </description>
-</model>

clover_simulation/models/square_line/square_line.sdf

@@ -1,24 +0,0 @@
-<?xml version="1.0"?>
-<sdf version="1.5">
-    <model name="square_line">
-        <static>true</static>
-        <link name="square_link">
-            <pose>0 0 1e-3 0 0 0</pose>
-            <visual name="square_texture">
-                <cast_shadows>false</cast_shadows>
-                <geometry>
-                    <box>
-                        <size>4.0 4.0 1e-3</size>
-                    </box>
-                </geometry>
-                <material>
-                    <script>
-                        <uri>model://square_line/materials/scripts</uri>
-                        <uri>model://square_line/materials/textures</uri>
-                        <name>square_solid</name>
-                    </script>
-                </material>
-            </visual>
-        </link>
-    </model>
-</sdf>

clover_simulation/package.xml

@@ -1,31 +0,0 @@
-<package format="2">
-    <name>clover_simulation</name>
-    <version>0.0.1</version>
-    <description>The clover_simulation package provides worlds and launch files for Gazebo.</description>
-
-    <maintainer email="okalachev@gmail.com">Oleg Kalachev</maintainer>
-
-    <author>Alexey Rogachevskiy</author>
-    <author>Andrey Ryabov</author>
-    <author>Arthur Golubtsov</author>
-    <author>Oleg Kalachev</author>
-    <author>Svyatoslav Zhuravlev</author>
-
-    <license>MIT</license>
-
-    <url type="website">https://github.com/CopterExpress/clover</url>
-    <url type="bugtracker">https://github.com/CopterExpress/clover/issues</url>
-
-    <buildtool_depend>catkin</buildtool_depend>
-    <depend>led_msgs</depend>
-    <depend>xacro</depend>
-    <depend>gazebo_ros</depend>
-    <depend>gazebo_plugins</depend>
-    <depend>rospy</depend>
-
-    <export>
-        <gazebo_ros gazebo_media_path="${prefix}"/>
-        <gazebo_ros gazebo_model_path="${prefix}/models"/>
-        <gazebo_ros gazebo_resource_path="${prefix}/resources"/>
-    </export>
-</package>

clover_simulation/resources/worlds/clover.world

@@ -1,42 +0,0 @@
-<?xml version="1.0" ?>
-<sdf version="1.5">
-  <world name="default">
-    <!-- A global light source -->
-    <include>
-      <uri>model://sun</uri>
-    </include>
-    <include>
-      <uri>model://parquet_plane</uri>
-    </include>
-
-    <scene>
-      <ambient>0.8 0.8 0.8 1</ambient>
-      <background>0.8 0.9 1 1</background>
-      <shadows>false</shadows>
-      <grid>false</grid>
-      <origin_visual>false</origin_visual>
-    </scene>
-  
-    <physics name='default_physics' default='0' type='ode'>
-      <gravity>0 0 -9.8066</gravity>
-      <ode>
-        <solver>
-          <type>quick</type>
-          <iters>10</iters>
-          <sor>1.3</sor>
-          <use_dynamic_moi_rescaling>0</use_dynamic_moi_rescaling>
-        </solver>
-        <constraints>
-          <cfm>0</cfm>
-          <erp>0.2</erp>
-          <contact_max_correcting_vel>100</contact_max_correcting_vel>
-          <contact_surface_layer>0.001</contact_surface_layer>
-        </constraints>
-      </ode>
-      <max_step_size>0.004</max_step_size>
-      <real_time_factor>1</real_time_factor>
-      <real_time_update_rate>250</real_time_update_rate>
-      <magnetic_field>6.0e-6 2.3e-5 -4.2e-5</magnetic_field>
-    </physics>
-  </world>
-</sdf>

clover_simulation/scripts/aruco_gen

@@ -1,6 +0,0 @@
-#! /usr/bin/env python
-
-from clover_simulation import aruco_gen
-
-if __name__ == "__main__":
-    aruco_gen()

clover_simulation/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
-
-setup_args = generate_distutils_setup(
-    packages=['clover_simulation'],
-    package_dir={'': 'src'}
-)
-
-setup(**setup_args)

clover_simulation/src/clover_simulation/__init__.py

@@ -1,93 +0,0 @@
-from docopt import docopt
-from os import path
-from sys import stdout
-
-from .map_parser import parse
-from .marker import Marker, generate_markers
-
-from .world import load_world, add_model, save_world
-
-
-USAGE = '''
-
-aruco_gen - Script for ArUco map/model generation.
-  Generates ArUco models from their description and optionally
-  adds them to an existing Gazebo world.
-
-Usage:
-  aruco_gen [--offset-x=<m>] [--offset-y=<m>] [--offset-z=<m>]
-            [--offset-roll=<rad>] [--offset-pitch=<rad>] [--offset-yaw=<rad>]
-            [--dictionary=<id>] [--single-model]
-            [--source-world=<path>] [--inplace]
-            [--model-path=<path>]
-            <aruco_map_file>
-  aruco_gen -h | --help
-
-Options:
-  -h, --help             Show this screen.
-  --offset-x=<m>         X offset in meters [default: 0].
-  --offset-y=<m>         Y offset in meters [default: 0].
-  --offset-z=<m>         Z offset in meters [default: 0].
-  --offset-roll=<rad>    roll offset in radians [default: 0].
-  --offset-pitch=<rad>   pitch offset in radians [default: 0].
-  --offset-yaw=<rad>     yaw offset in radians [default: 0].
-  --dictionary=<id>      ArUco dictionary ID [default: 2].
-  --single-model         Generate a single model instead of individual
-                         markers.
-  --source-world=<path>  Path to existing Gazebo world.
-  --inplace              Modify source world.
-  --model-path=<path>    Folder where generated models will be saved
-                         [default: ~/.gazebo/models]
-  aruco_map_file         Full path to the ArUco map file
-
-'''
-
-
-def aruco_gen():
-    opts = docopt(USAGE)
-
-    dictionary_id = int(opts['--dictionary'])
-    mapfile = opts['<aruco_map_file>']
-    single_model = opts['--single-model']
-    source_world = opts['--source-world']
-    inplace = opts['--inplace']
-
-    off_x = float(opts['--offset-x'])
-    off_y = float(opts['--offset-y'])
-    off_z = float(opts['--offset-z'])
-    off_roll = float(opts['--offset-roll'])
-    off_pitch = float(opts['--offset-pitch'])
-    off_yaw = float(opts['--offset-yaw'])
-
-    model_base_path = path.expanduser(opts['--model-path'])
-
-    markers = parse(mapfile)
-
-    if single_model:
-        mapname = path.split(mapfile)[-1]
-        model_path = path.join(model_base_path, 'aruco_{}'.format(mapname.replace('.', '_')))
-        generate_markers(markers, model_path, dictionary_id=dictionary_id, map_source=mapname)
-    else:
-        for marker in markers:
-            model_name = 'aruco_{}_{}'.format(dictionary_id, marker.id_)
-            model_path = path.join(model_base_path, model_name)
-            generate_markers([Marker(marker.id_, marker.size, 0, 0, 0, 0, 0, 0)],
-                model_path, dictionary_id=dictionary_id)
-
-    if source_world is not None:
-        world_tree = load_world(source_world)
-        if single_model:
-            world_tree = add_model(world_tree, 'aruco_{}'.format(mapname.replace('.', '_')),
-                off_x, off_y, off_z,
-                off_roll, off_pitch, off_yaw)
-        else:
-            if (abs(off_roll) > 0.001) or (abs(off_pitch) > 0.001) or (abs(off_yaw) > 0.001):
-                raise NotImplementedError('Sorry, angular offsets are not currently implemented for multimodel generation')
-            for marker in markers:
-                world_tree = add_model(world_tree, 'aruco_{}_{}'.format(dictionary_id, marker.id_),
-                    off_x + marker.x, off_y + marker.y, off_z + marker.z,
-                    marker.roll, marker.pitch, marker.yaw)
-
-        output = open(source_world, 'w') if inplace else stdout
-
-        save_world(world_tree, output)