CopterExpress/clover
1
0
Fork 0
mirror of https://github.com/CopterExpress/clover.git synced 2026-05-26 11:43:25 +00:00
Code Issues Packages Projects Releases Wiki Activity

Merge branch 'master' into roswww-static-target

Browse Source
This commit is contained in:
Oleg Kalachev
2022-11-04 03:51:51 +06:00
parent 9cae4c9064 2b13aa02eb
commit 873a08865e
241 changed files with 4272 additions and 852 deletions
Download Patch File Download Diff File Expand all files Collapse all files

1
.gitattributes vendored
View File

@@ -3,6 +3,7 @@ roslib.js linguist-vendored
eventemitter2.js linguist-vendored
ros3d.js linguist-vendored
three.min.js linguist-vendored
json-to-pretty-yaml.js linguist-vendored
aruco_pose/vendor/* linguist-vendored
blockly/* linguist-vendored
highlight/* linguist-vendored

50
.github/workflows/build.yml vendored
View File

@@ -7,17 +7,45 @@ on:
branches: [ master ]
jobs:
melodic:
runs-on: ubuntu-latest
steps:
- uses: actions/checkout@v2
- name: Native Melodic build
run: |
docker run --rm -v $(pwd):/root/catkin_ws/src/clover ros:melodic-ros-base /root/catkin_ws/src/clover/builder/standalone-install.sh
# melodic:
# runs-on: ubuntu-latest
# steps:
# - uses: actions/checkout@v2
# - name: Native Melodic build
# run: |
# docker run --rm -v $(pwd):/root/catkin_ws/src/clover ros:melodic-ros-base /root/catkin_ws/src/clover/builder/standalone-install.sh
noetic:
runs-on: ubuntu-latest
container: ros:noetic-ros-base
defaults:
run:
working-directory: catkin_ws
shell: bash
steps:
- uses: actions/checkout@v2
- name: Native Noetic build
run: |
docker run --rm -v $(pwd):/root/catkin_ws/src/clover ros:noetic-ros-base /root/catkin_ws/src/clover/builder/standalone-install.sh
- uses: actions/checkout@v2
with:
path: catkin_ws/src/clover
- name: Install requirements
run: apt-get update && apt-get -y install python3-pip fakeroot python3-bloom debhelper dpkg-dev
- name: Install dependencies
run: rosdep update && rosdep install --from-paths src --ignore-src -y
- name: Install GeographicLib datasets
run: wget -qO- https://raw.githubusercontent.com/mavlink/mavros/master/mavros/scripts/install_geographiclib_datasets.sh | bash
- name: catkin_make
run: source /opt/ros/$ROS_DISTRO/setup.bash && catkin_make
- name: Run tests
run: source devel/setup.bash && catkin_make run_tests && catkin_test_results
- name: Build Debian packages
run: |
source devel/setup.bash
for file in `find . -name "package.xml"`; do
cd $(dirname ${file})
bloom-generate rosdebian --os-name ubuntu --os-version $(lsb_release -cs) --ros-distro $ROS_DISTRO
fakeroot debian/rules binary
cd -
done
- uses: actions/upload-artifact@v3
with:
name: debian-packages
path: catkin_ws/src/clover/*.deb
retention-days: 1

53
.github/workflows/docs.yml vendored
View File

@@ -4,11 +4,24 @@ on:
push:
branches: [ '*' ]
pull_request:
branches: [ master ]
branches: [ '*' ]
permissions:
contents: read
pages: write
id-token: write
concurrency:
group: "pages"
cancel-in-progress: true
defaults:
run:
shell: bash
jobs:
docs:
runs-on: ubuntu-18.04
runs-on: ubuntu-22.04
steps:
- uses: actions/checkout@v2
- name: Use Node.js
@@ -34,7 +47,11 @@ jobs:
gitbook install
gitbook build
- name: Generate PDF
if: ${{ github.event_name == 'push' && github.ref == 'refs/heads/master' }}
id: generate-pdf
env:
GITBOOK_SKIP_PDF: ${{ secrets.GITBOOK_SKIP_PDF }}
continue-on-error: ${{ env.GITBOOK_SKIP_PDF != '' }}
if: ${{ github.event_name == 'push' }}
run: |
for i in 1 2 3 4; do gitbook pdf ./ _book/clover.pdf && break || sleep 1; done
sudo apt-get -q install ghostscript
@@ -43,11 +60,27 @@ jobs:
rm _book/clover_ru.pdf && mv _book/clover_ru_compressed.pdf _book/clover_ru.pdf
rm _book/clover_en.pdf && mv _book/clover_en_compressed.pdf _book/clover_en.pdf
ls -lah _book/clover*.pdf
- name: Deploy
uses: JamesIves/github-pages-deploy-action@4.1.3
if: ${{ github.event_name == 'push' && github.ref == 'refs/heads/master' }}
echo '::set-output name=GITBOOK_PDF_OK::1'
- name: Download older PDFs
if: ${{ !steps.generate-pdf.outputs.GITBOOK_PDF_OK }}
run: |
rm -f _book/clover*.pdf
wget --no-verbose https://clover.coex.tech/clover_ru.pdf -P _book/
wget --no-verbose https://clover.coex.tech/clover_en.pdf -P _book/
- name: Upload artifact
# if: ${{ github.event_name == 'push' && github.ref == 'refs/heads/master' }}
uses: actions/upload-pages-artifact@v1
with:
branch: gh-pages
folder: _book
clean: true
single-commit: true # to avoid multiple copies of large pdf files
path: _book
deploy-docs:
if: ${{ github.event_name == 'push' && github.ref == 'refs/heads/master' }}
environment:
name: github-pages
url: ${{ steps.deployment.outputs.page_url }}
runs-on: ubuntu-latest
needs: docs
steps:
- name: Deploy to GitHub Pages
id: deployment
uses: actions/deploy-pages@v1

11
aruco_pose/CMakeLists.txt
View File

@@ -83,11 +83,10 @@ add_message_files(
)
## Generate services in the 'srv' folder
# add_service_files(
# FILES
# Service1.srv
# Service2.srv
# )
add_service_files(
FILES
SetMarkers.srv
)
## Generate actions in the 'action' folder
# add_action_files(
@@ -120,6 +119,7 @@ generate_messages(
## Generate dynamic reconfigure parameters in the 'cfg' folder
generate_dynamic_reconfigure_options(
cfg/Detector.cfg
cfg/Map.cfg
)
###################################
@@ -251,4 +251,5 @@ if (CATKIN_ENABLE_TESTING)
add_rostest(test/test_node_failure.test)
add_rostest(test/largemap.test)
add_rostest(test/crash_opencv.test)
add_rostest(test/duplicate.test)
endif()

3
aruco_pose/README.md
View File

@@ -51,6 +51,7 @@ It's recommended to run it within the same nodelet manager with the camera nodel
* `image_raw` (*sensor_msgs/Image*) – camera image
* `camera_info` (*sensor_msgs/CameraInfo*) – camera calibration info
* `map_markers` (*aruco_pose/MarkerArray*) – list of markers to disable TF transform publishing
#### Published
@@ -74,6 +75,7 @@ It's recommended to run it within the same nodelet manager with the camera nodel
* `~image_width` – debug image width (default: 2000)
* `~image_height` – debug image height (default: 2000)
* `~image_margin` – debug image margin (default: 200)
* `~image_axis` – whether debug image should contain axis (default: true)
* `~dictionary` (*int*) – ArUco dictionary (default: 2) - should be the same as `dictionary` parameter of `aruco_detect` nodelet
Map file has one marker per line with the following line format:
@@ -97,6 +99,7 @@ See examples in [`map`](map/) directory.
#### Published
* `~pose` (*geometry_msgs/PoseWithCovarianceStamped*) – estimated map pose
* `~map` (*aruco_pose/MarkerArray*) – list of markers in the loaded map
* `~image` (*sensor_msgs/Image*) – planarized map image
* `~visualization` (*visualization_msgs/MarkerArray*) – markers map visualization for rviz
* `~debug` (*sensor_msgs/Image*) – debug image with detected markers and map axis

2
aruco_pose/cfg/Detector.cfg
View File

@@ -10,6 +10,8 @@ gen = ParameterGenerator()
gen.add("enabled", bool_t, 0, "if detection enabled", True)
gen.add("length", double_t, 0, "markers' side length", min=0, max=10)
gen.add("adaptiveThreshConstant", double_t, 0,
"Constant for adaptive thresholding before finding contours",
p.adaptiveThreshConstant, 0, 100)

14
aruco_pose/cfg/Map.cfg Normal file
View File

@@ -0,0 +1,14 @@
#!/usr/bin/env python
PACKAGE = "aruco_pose"
from dynamic_reconfigure.parameter_generator_catkin import *
gen = ParameterGenerator()
gen.add("enabled", bool_t, 0, "if map detection enabled", True)
gen.add("map", str_t, 0, "full path for the map file")
gen.add("image_axis", bool_t, 0, "debug image axis", default=True)
exit(gen.generate(PACKAGE, "aruco_pose", "Map"))

1
aruco_pose/map/cmit.txt
View File

@@ -1,3 +1,4 @@
# id length x y z rot_z rot_y rot_x
0 0.33 0.0 9.0 0 0 0 0
1 0.33 1.0 9.0 0 0 0 0
2 0.33 2.0 9.0 0 0 0 0

1
aruco_pose/map/office.txt
View File

@@ -1,3 +1,4 @@
# id length x y z rot_z rot_y rot_x
107 0.33 0 0 0 0 0 0
106 0.33 0.77 0 0 0 0 0
105 0.33 0 0.77 0 0 0 0

1
aruco_pose/map/office_ceiling.txt
View File

@@ -1,3 +1,4 @@
# id length x y z rot_z rot_y rot_x
14 0.365 0.000 0.0 0 0 0 0
15 0.365 1.335 0.0 0 0 0 0
30 0.365 2.865 0.0 0 0 0 0

2
aruco_pose/package.xml
View File

@@ -1,7 +1,7 @@
<?xml version="1.0"?>
<package format="2">
<name>aruco_pose</name>
<version>0.21.1</version>
<version>0.23.0</version>
<description>Positioning with ArUco markers</description>
<maintainer email="okalachev@gmail.com">Oleg Kalachev</maintainer>

69
aruco_pose/src/aruco_detect.cpp
View File

@@ -48,6 +48,7 @@
#include <aruco_pose/Marker.h>
#include <aruco_pose/MarkerArray.h>
#include <aruco_pose/DetectorConfig.h>
#include <aruco_pose/SetMarkers.h>
#include "utils.h"
#include <memory>
@@ -69,8 +70,11 @@ private:
image_transport::CameraSubscriber img_sub_;
ros::Publisher markers_pub_, vis_markers_pub_;
ros::Subscriber map_markers_sub_;
bool estimate_poses_, send_tf_, auto_flip_;
ros::ServiceServer set_markers_srv_;
bool estimate_poses_, send_tf_, auto_flip_, use_map_markers_;
bool waiting_for_map_;
double length_;
ros::Duration transform_timeout_;
std::unordered_map<int, double> length_override_;
std::string frame_id_prefix_, known_tilt_;
Mat camera_matrix_, dist_coeffs_;
@@ -92,11 +96,14 @@ public:
dictionary = nh_priv_.param("dictionary", 2);
estimate_poses_ = nh_priv_.param("estimate_poses", true);
send_tf_ = nh_priv_.param("send_tf", true);
use_map_markers_ = nh_priv_.param("use_map_markers", false);
waiting_for_map_ = use_map_markers_;
if (estimate_poses_ && !nh_priv_.getParam("length", length_)) {
NODELET_FATAL("can't estimate marker's poses as ~length parameter is not defined");
ros::shutdown();
}
readLengthOverride(nh_priv_);
transform_timeout_ = ros::Duration(nh_priv_.param("transform_timeout", 0.02));
known_tilt_ = nh_priv_.param<std::string>("known_tilt", "");
auto_flip_ = nh_priv_.param("auto_flip", false);
@@ -114,6 +121,8 @@ public:
dyn_srv_ = std::make_shared<dynamic_reconfigure::Server<aruco_pose::DetectorConfig>>(nh_priv_);
dyn_srv_->setCallback(std::bind(&ArucoDetect::paramCallback, this, std::placeholders::_1, std::placeholders::_2));
set_markers_srv_ = nh_priv_.advertiseService("set_length_override", &ArucoDetect::setMarkers, this);
debug_pub_ = it_priv.advertise("debug", 1);
markers_pub_ = nh_priv_.advertise<aruco_pose::MarkerArray>("markers", 1);
vis_markers_pub_ = nh_priv_.advertise<visualization_msgs::MarkerArray>("visualization", 1);
@@ -127,6 +136,7 @@ private:
void imageCallback(const sensor_msgs::ImageConstPtr& msg, const sensor_msgs::CameraInfoConstPtr &cinfo)
{
if (!enabled_) return;
if (waiting_for_map_) return;
Mat image = cv_bridge::toCvShare(msg, "bgr8")->image;
@@ -172,7 +182,7 @@ private:
if (!known_tilt_.empty()) {
try {
snap_to = tf_buffer_->lookupTransform(msg->header.frame_id, known_tilt_,
msg->header.stamp, ros::Duration(0.02));
msg->header.stamp, transform_timeout_);
} catch (const tf2::TransformException& e) {
NODELET_WARN_THROTTLE(5, "can't snap: %s", e.what());
}
@@ -181,6 +191,8 @@ private:
array_.markers.reserve(ids.size());
aruco_pose::Marker marker;
vector<geometry_msgs::TransformStamped> transforms;
transforms.reserve(ids.size());
geometry_msgs::TransformStamped transform;
transform.header.stamp = msg->header.stamp;
transform.header.frame_id = msg->header.frame_id;
@@ -198,20 +210,33 @@ private:
snapOrientation(marker.pose.orientation, snap_to.transform.rotation, auto_flip_);
}
// TODO: check IDs are unique
if (send_tf_) {
transform.child_frame_id = getChildFrameId(ids[i]);
// check if such static transform is in the map
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);
// check if a markers with that id is already added
bool send = true;
for (auto &t : transforms) {
if (t.child_frame_id == transform.child_frame_id) {
send = false;
break;
}
}
if (send) {
transform.transform.rotation = marker.pose.orientation;
fillTranslation(transform.transform.translation, tvecs[i]);
transforms.push_back(transform);
}
}
}
}
array_.markers.push_back(marker);
}
if (send_tf_) {
br_->sendTransform(transforms);
}
}
markers_pub_.publish(array_);
@@ -346,17 +371,47 @@ private:
}
}
bool setMarkers(aruco_pose::SetMarkers::Request& req, aruco_pose::SetMarkers::Response& res)
{
for (auto const& marker : req.markers) {
if (marker.id > 999) {
res.message = "Invalid marker id: " + std::to_string(marker.id);
ROS_ERROR("%s", res.message.c_str());
return true;
}
if (!std::isfinite(marker.length) || marker.length <= 0) {
res.message = "Invalid marker " + std::to_string(marker.id) + " length: " + std::to_string(marker.length);
ROS_ERROR("%s", res.message.c_str());
return true;
}
}
for (auto const& marker : req.markers) {
length_override_[marker.id] = marker.length;
}
res.success = true;
return true;
}
void mapMarkersCallback(const aruco_pose::MarkerArray& msg)
{
map_markers_ids_.clear();
for (auto const& marker : msg.markers) {
map_markers_ids_.insert(marker.id);
if (use_map_markers_) {
if (length_override_.find(marker.id) == length_override_.end()) {
length_override_[marker.id] = marker.length;
}
}
}
waiting_for_map_ = false;
}
void paramCallback(aruco_pose::DetectorConfig &config, uint32_t level)
{
enabled_ = config.enabled;
enabled_ = config.enabled && config.length > 0;
length_ = config.length;
parameters_->adaptiveThreshConstant = config.adaptiveThreshConstant;
parameters_->adaptiveThreshWinSizeMin = config.adaptiveThreshWinSizeMin;
parameters_->adaptiveThreshWinSizeMax = config.adaptiveThreshWinSizeMax;

89
aruco_pose/src/aruco_map.cpp
View File

@@ -19,11 +19,13 @@
#include <vector>
#include <fstream>
#include <algorithm>
#include <memory>
#include <ros/ros.h>
#include <nodelet/nodelet.h>
#include <pluginlib/class_list_macros.h>
#include <image_transport/image_transport.h>
#include <cv_bridge/cv_bridge.h>
#include <dynamic_reconfigure/server.h>
#include <tf/transform_datatypes.h>
#include <tf2_ros/buffer.h>
#include <tf2_ros/transform_listener.h>
@@ -41,6 +43,7 @@
#include <aruco_pose/MarkerArray.h>
#include <aruco_pose/Marker.h>
#include <aruco_pose/MapConfig.h>
#include <opencv2/opencv.hpp>
#include <opencv2/aruco.hpp>
@@ -74,6 +77,9 @@ private:
tf2_ros::StaticTransformBroadcaster static_br_;
tf2_ros::Buffer tf_buffer_;
tf2_ros::TransformListener tf_listener_{tf_buffer_};
std::shared_ptr<dynamic_reconfigure::Server<aruco_pose::MapConfig>> dyn_srv_;
bool enabled_ = true;
std::string type_;
visualization_msgs::MarkerArray vis_array_;
std::string known_tilt_, map_, markers_frame_, markers_parent_frame_;
int image_width_, image_height_, image_margin_;
@@ -89,15 +95,14 @@ public:
// TODO: why image_transport doesn't work here?
img_pub_ = nh_priv_.advertise<sensor_msgs::Image>("image", 1, true);
markers_pub_ = nh_priv_.advertise<aruco_pose::MarkerArray>("markers", 1, true);
markers_pub_ = nh_priv_.advertise<aruco_pose::MarkerArray>("map", 1, true);
board_ = cv::makePtr<cv::aruco::Board>();
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);
std::string type, map;
type = nh_priv_.param<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);
@@ -110,13 +115,13 @@ public:
// createStripLine();
if (type == "map") {
param(nh_priv_, "map", map);
loadMap(map);
} else if (type == "gridboard") {
if (type_ == "map") {
map_ = nh_priv_.param<std::string>("map" , "");
loadMap(map_);
} else if (type_ == "gridboard") {
createGridBoard(nh_priv_);
} else {
NODELET_FATAL("unknown type: %s", type.c_str());
NODELET_FATAL("unknown type: %s", type_.c_str());
ros::shutdown();
}
@@ -124,10 +129,7 @@ public:
vis_markers_pub_ = nh_priv_.advertise<visualization_msgs::MarkerArray>("visualization", 1, true);
debug_pub_ = it_priv.advertise("debug", 1);
publishMarkersFrames();
publishMarkers();
publishMapImage();
vis_markers_pub_.publish(vis_array_);
publishMap();
image_sub_.subscribe(nh_, "image_raw", 1);
info_sub_.subscribe(nh_, "camera_info", 1);
@@ -136,6 +138,12 @@ public:
sync_.reset(new message_filters::Synchronizer<SyncPolicy>(SyncPolicy(10), image_sub_, info_sub_, markers_sub_));
sync_->registerCallback(boost::bind(&ArucoMap::callback, this, _1, _2, _3));
dyn_srv_ = std::make_shared<dynamic_reconfigure::Server<aruco_pose::MapConfig>>(nh_priv_);
dynamic_reconfigure::Server<aruco_pose::MapConfig>::CallbackType cb;
cb = std::bind(&ArucoMap::paramCallback, this, std::placeholders::_1, std::placeholders::_2);
dyn_srv_->setCallback(cb);
NODELET_INFO("ready");
}
@@ -143,6 +151,9 @@ public:
const sensor_msgs::CameraInfoConstPtr& cinfo,
const aruco_pose::MarkerArrayConstPtr& markers)
{
if (!enabled_) return;
if (markers->markers.empty()) return; // map not loaded
int valid = 0;
int count = markers->markers.size();
std::vector<int> ids;
@@ -268,9 +279,17 @@ publish_debug:
std::ifstream f(filename);
std::string line;
clearMarkers();
if (map_ == "") {
NODELET_INFO("No map loaded");
return;
}
if (!f.good()) {
NODELET_FATAL("%s - %s", strerror(errno), filename.c_str());
ros::shutdown();
NODELET_ERROR("%s - %s", strerror(errno), filename.c_str());
map_ = "";
return;
}
while (std::getline(f, line)) {
@@ -296,9 +315,10 @@ publish_debug:
s.putback(first);
} else {
// Probably garbage data; inform user and throw an exception, possibly killing nodelet
NODELET_FATAL("Malformed input: %s", line.c_str());
ros::shutdown();
throw std::runtime_error("Malformed input");
NODELET_ERROR("Malformed input: %s", line.c_str());
map_ = "";
clearMarkers();
return;
}
if (!(s >> id >> length >> x >> y)) {
@@ -329,6 +349,14 @@ publish_debug:
NODELET_INFO("loading %s complete (%d markers)", filename.c_str(), static_cast<int>(board_->ids.size()));
}
void publishMap()
{
publishMarkersFrames();
publishMarkers();
publishMapImage();
vis_markers_pub_.publish(vis_array_);
}
void createGridBoard(ros::NodeHandle& nh)
{
NODELET_INFO("generate gridboard");
@@ -370,6 +398,15 @@ publish_debug:
}
}
void clearMarkers()
{
board_->ids.clear();
board_->objPoints.clear();
markers_.markers.clear();
vis_array_.markers.clear();
markers_transforms_.clear();
}
// void createStripLine()
// {
// visualization_msgs::Marker marker;
@@ -466,7 +503,7 @@ publish_debug:
vis_marker.pose.position.x = x;
vis_marker.pose.position.y = y;
vis_marker.pose.position.z = z;
tf::quaternionTFToMsg(q, marker.pose.orientation);
tf::quaternionTFToMsg(q, vis_marker.pose.orientation);
vis_marker.frame_locked = true;
vis_array_.markers.push_back(vis_marker);
@@ -509,6 +546,22 @@ publish_debug:
msg.image = image;
img_pub_.publish(msg.toImageMsg());
}
void paramCallback(aruco_pose::MapConfig &config, uint32_t level)
{
// https://github.com/CopterExpress/clover/commit/2cd334c474e3ed04ef65ca1ba7f08ab535a3dc6d#diff-942723f9452c398ae93f1a91427f9a7b614be5e5871f8a3e590f324d804f0d58R356
enabled_ = config.enabled;
if (type_ == "map" && config.map != map_) {
map_ = config.map;
loadMap(map_);
publishMap();
}
if (config.image_axis != image_axis_) {
image_axis_ = config.image_axis;
publishMapImage();
}
}
};
PLUGINLIB_EXPORT_CLASS(ArucoMap, nodelet::Nodelet)

2
aruco_pose/src/genmap.py
View File

@@ -30,7 +30,7 @@ Options:
-o <filename> Output map file name in the 'map' subdirectory of aruco_pose package
Example:
rosrun aruco_pose genmap.py 0.33 2 4 1 1 0 > $(catkin_find aruco_pose map)/test_map.txt
rosrun aruco_pose genmap.py 0.33 2 4 1 1 0 -o test_map.txt
"""
from __future__ import print_function

7
aruco_pose/srv/SetMarkers.srv Normal file
View File

@@ -0,0 +1,7 @@
# * Add or change markers in the map
# * Change markers' properties, e. g. lengths
Marker[] markers # if length or pose is nan - remove from map
---
bool success
string message

34
aruco_pose/test/basic.py
View File

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

BIN
aruco_pose/test/duplicate.png Normal file
View File

Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 62 KiB

8
aruco_pose/test/duplicate.py Normal file
View File

@@ -0,0 +1,8 @@
import pytest
import subprocess
def test_no_tf_repeated_data():
# `/rosout` acts weirdly inside rostest, so using a subprocess
cmd = """python -c 'import rospy, tf; rospy.init_node("foo"); listener = tf.TransformListener(); rospy.sleep(2)'"""
output = str(subprocess.check_output(cmd, shell=True, stderr=subprocess.STDOUT))
assert 'TF_REPEATED_DATA' not in output, 'TF_REPEATED_DATA was logged on duplicate markers'

21
aruco_pose/test/duplicate.test Normal file
View File

@@ -0,0 +1,21 @@
<launch>
<node pkg="image_publisher" type="image_publisher" name="main_camera" args="$(find aruco_pose)/test/duplicate.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" args="manager" required="true"/>
<node pkg="nodelet" clear_params="true" type="nodelet" name="aruco_detect" args="load aruco_pose/aruco_detect nodelet_manager" required="true">
<remap from="image_raw" to="main_camera/image_raw"/>
<remap from="camera_info" to="main_camera/camera_info"/>
<param name="length" value="0.33"/>
<param name="estimate_poses" value="true"/>
<param name="send_tf" value="true"/>
<param name="cornerRefinementMethod" value="1"/>
</node>
<param name="test_module" value="$(find aruco_pose)/test/duplicate.py"/>
<test test-name="aruco_pose_test" pkg="ros_pytest" type="ros_pytest_runner"/>
</launch>

5
aruco_pose/test/test_node_failure.py
View File

@@ -2,6 +2,7 @@ import rospy
import pytest
from visualization_msgs.msg import MarkerArray as VisMarkerArray
from aruco_pose.msg import MarkerArray
@pytest.fixture
@@ -9,5 +10,5 @@ def node():
return rospy.init_node('aruco_pose_test', anonymous=True)
def test_node_failure(node):
with pytest.raises(rospy.exceptions.ROSException):
rospy.wait_for_message('aruco_map/visualization', VisMarkerArray, timeout=5)
assert rospy.wait_for_message('aruco_map/visualization', VisMarkerArray, timeout=5).markers == []
assert rospy.wait_for_message('aruco_map/map', MarkerArray, timeout=5).markers == []

2
builder/image-ros.sh
View File

@@ -90,7 +90,7 @@ echo_stamp "Installing OpenCV 4.2-compatible ROS packages"
apt install -y --no-install-recommends \
ros-${ROS_DISTRO}-compressed-image-transport=1.14.0-0buster \
ros-${ROS_DISTRO}-cv-bridge=1.15.0-0buster \
ros-${ROS_DISTRO}-cv-camera=0.5.0-0buster \
ros-${ROS_DISTRO}-cv-camera=0.5.1-0buster \
ros-${ROS_DISTRO}-image-publisher=1.15.3-0buster \
ros-${ROS_DISTRO}-web-video-server=0.2.1-0buster
apt-mark hold \

1
builder/image-software.sh
View File

@@ -138,6 +138,7 @@ echo_stamp "Install and enable Butterfly (web terminal)"
echo_stamp "Workaround for tornado >= 6.0 breaking butterfly"
export CRYPTOGRAPHY_DONT_BUILD_RUST=1
my_travis_retry pip3 install cryptography==3.4.6 # https://stackoverflow.com/a/68472128/6850197
my_travis_retry pip3 install pyOpenSSL==20.0.1
my_travis_retry pip3 install tornado==5.1.1
my_travis_retry pip3 install butterfly
my_travis_retry pip3 install butterfly[systemd]

1
builder/test/tests.py
View File

@@ -22,6 +22,7 @@ from clover.srv import GetTelemetry, Navigate, NavigateGlobal, SetPosition, SetV
from led_msgs.srv import SetLEDs
from led_msgs.msg import LEDStateArray, LEDState
from aruco_pose.msg import Marker, MarkerArray, Point2D
from clover import long_callback
import dynamic_reconfigure.client

4
builder/test/tests.sh
View File

@@ -58,6 +58,10 @@ rosversion rosshow
rosversion nodelet
rosversion image_view
# validate some versions
[[ $(rosversion cv_camera) == "0.5.1" ]] # patched version with init fix
[[ $(rosversion ws281x) == "0.0.13" ]]
# validate examples are present
[[ $(ls /home/pi/examples/*) ]]

13
clover/CMakeLists.txt
View File

@@ -17,6 +17,7 @@ find_package(catkin REQUIRED COMPONENTS
message_generation
geometry_msgs
sensor_msgs
led_msgs
geographic_msgs
tf
tf2
@@ -24,6 +25,7 @@ find_package(catkin REQUIRED COMPONENTS
tf2_ros
image_transport
cv_bridge
dynamic_reconfigure
)
list(APPEND CMAKE_MODULE_PATH "${CMAKE_CURRENT_LIST_DIR}/cmake")
@@ -51,7 +53,7 @@ find_package(OpenCV ${_opencv_version} REQUIRED
## Uncomment this if the package has a setup.py. This macro ensures
## modules and global scripts declared therein get installed
## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html
# catkin_python_setup()
catkin_python_setup()
################################################
## Declare ROS messages, services and actions ##
@@ -126,10 +128,9 @@ generate_messages(
## and list every .cfg file to be processed
## Generate dynamic reconfigure parameters in the 'cfg' folder
# generate_dynamic_reconfigure_options(
# cfg/DynReconf1.cfg
# cfg/DynReconf2.cfg
# )
generate_dynamic_reconfigure_options(
cfg/Flow.cfg
)
###################################
## catkin specific configuration ##
@@ -211,6 +212,8 @@ add_dependencies(clover_led ${PROJECT_NAME}_generate_messages_cpp)
add_dependencies(shell ${PROJECT_NAME}_generate_messages_cpp)
add_dependencies(${PROJECT_NAME} ${PROJECT_NAME}_gencfg)
## Rename C++ executable without prefix
## The above recommended prefix causes long target names, the following renames the
## target back to the shorter version for ease of user use

10
clover/cfg/Flow.cfg Normal file
View File

@@ -0,0 +1,10 @@
#!/usr/bin/env python
PACKAGE = "clover"
from dynamic_reconfigure.parameter_generator_catkin import *
gen = ParameterGenerator()
gen.add("enabled", bool_t, 0, "if optical flow enabled", True)
exit(gen.generate(PACKAGE, "clover", "Flow"))

19
clover/launch/aruco.launch
View File

@@ -8,16 +8,21 @@
<!-- For additional help go to https://clover.coex.tech/aruco -->
<arg name="force_init" default="false"/>
<arg name="disable" default="false"/> <!-- only force init -->
<!-- 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 main_camera_nodelet_manager" output="screen" clear_params="true" respawn="true">
<node name="aruco_detect" pkg="nodelet" if="$(eval aruco_detect and not disable)" type="nodelet" args="load aruco_pose/aruco_detect main_camera_nodelet_manager" output="screen" clear_params="true" respawn="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"/>
<remap from="map_markers" to="aruco_map/map"/>
<param name="estimate_poses" value="true"/>
<param name="send_tf" value="true"/>
<param name="use_map_markers" value="true"/>
<param name="known_tilt" value="map" if="$(eval placement == 'floor')"/>
<param name="known_tilt" value="map_flipped" if="$(eval placement == 'ceiling')"/>
<param name="length" value="$(arg length)"/>
<param name="transform_timeout" value="0.1"/>
<!-- aruco detector parameters -->
<param name="cornerRefinementMethod" value="2"/> <!-- contour refinement -->
<param name="minMarkerPerimeterRate" value="0.075"/> <!-- 0.075 for 320x240, 0.0375 for 640x480 -->
@@ -26,7 +31,7 @@
</node>
<!-- aruco_map: estimate aruco map pose -->
<node name="aruco_map" pkg="nodelet" type="nodelet" if="$(arg aruco_map)" args="load aruco_pose/aruco_map main_camera_nodelet_manager" output="screen" clear_params="true" respawn="true">
<node name="aruco_map" pkg="nodelet" type="nodelet" if="$(eval aruco_map and not disable)" args="load aruco_pose/aruco_map main_camera_nodelet_manager" output="screen" clear_params="true" respawn="true">
<remap from="image_raw" to="main_camera/image_raw"/>
<remap from="camera_info" to="main_camera/camera_info"/>
<remap from="markers" to="aruco_detect/markers"/>
@@ -41,11 +46,11 @@
</node>
<!-- vpe publisher from aruco markers -->
<node name="vpe_publisher" pkg="clover" type="vpe_publisher" if="$(arg aruco_vpe)" output="screen" clear_params="true">
<remap from="~pose_cov" to="aruco_map/pose"/>
<node name="vpe_publisher" pkg="clover" type="vpe_publisher" if="$(eval aruco_vpe or force_init)" output="screen" clear_params="true">
<remap from="~pose_cov" to="aruco_map/pose" if="$(arg aruco_vpe)"/>
<remap from="~vpe" to="mavros/vision_pose/pose"/>
<param name="frame_id" value="aruco_map_detected"/>
<param name="publish_zero" value="true"/>
<param name="frame_id" value="aruco_map_detected" if="$(arg aruco_vpe)"/>
<param name="force_init" value="$(arg force_init)"/>
<param name="offset_frame_id" value="aruco_map"/>
</node>
</launch>

10
clover/launch/clover.launch
View File

@@ -12,6 +12,7 @@
<arg name="led" default="true"/>
<arg name="blocks" default="false"/>
<arg name="rc" default="false"/>
<arg name="force_init" default="true"/> <!-- force estimator to init by publishing zero pose -->
<arg name="simulator" default="false"/> <!-- flag that we are operating on a simulated drone -->
@@ -33,7 +34,10 @@
</node>
<!-- aruco markers -->
<include file="$(find clover)/launch/aruco.launch" if="$(arg aruco)"/>
<include file="$(find clover)/launch/aruco.launch" if="$(eval aruco or force_init)">
<arg name="force_init" value="$(arg force_init)"/>
<arg name="disable" value="$(eval not aruco)"/>
</include>
<!-- optical flow -->
<node pkg="nodelet" type="nodelet" name="optical_flow" args="load clover/optical_flow main_camera_nodelet_manager" if="$(arg optical_flow)" clear_params="true" output="screen" respawn="true">
@@ -41,15 +45,13 @@
<remap from="camera_info" to="main_camera/camera_info"/>
<param name="calc_flow_gyro" value="true"/>
<param name="roi_rad" value="0.8"/>
<param name="disable_on_vpe" value="false"/>
</node>
<node pkg="tf2_ros" type="static_transform_publisher" name="map_flipped_frame" args="0 0 0 3.1415926 3.1415926 0 map map_flipped"/>
<!-- simplified offboard control -->
<node name="simple_offboard" pkg="clover" type="simple_offboard" output="screen" clear_params="true">
<param name="reference_frames/body" value="map"/>
<param name="reference_frames/base_link" value="map"/>
<param name="reference_frames/navigate_target" value="map"/>
<param name="reference_frames/main_camera_optical" value="map"/>
</node>

6
clover/launch/main_camera.launch
View File

@@ -4,6 +4,8 @@
<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="device" default="/dev/video0"/> <!-- v4l2 device -->
<arg name="throttled_topic" default="true"/> <!-- enable throttled image topic -->
<arg name="throttled_topic_rate" default="5.0"/> <!-- throttled image topic rate -->
<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"/>
@@ -43,4 +45,8 @@
<node pkg="clover" type="camera_markers" ns="main_camera" name="main_camera_markers">
<param name="scale" value="3.0"/>
</node>
<!-- image topic throttled -->
<node pkg="topic_tools" name="main_camera_throttle" type="throttle" ns="main_camera"
args="messages image_raw $(arg throttled_topic_rate) image_raw_throttled" if="$(arg throttled_topic)"/>
</launch>

7
clover/launch/mavros.launch
View File

@@ -1,5 +1,5 @@
<launch>
<arg name="fcu_conn" default="usb"/> <!-- options: usb, uart, tcp, udp, sitl -->
<arg name="fcu_conn" default="usb"/> <!-- options: usb, uart, tcp, udp, sitl, hitl -->
<arg name="fcu_ip" default="127.0.0.1"/>
<arg name="fcu_sys_id" default="1"/>
<arg name="gcs_bridge" default="tcp"/>
@@ -23,6 +23,9 @@
<!-- sitl since PX4 1.9.0 -->
<param name="fcu_url" value="udp://@$(arg fcu_ip):14580" if="$(eval fcu_conn == 'sitl')"/>
<!-- hitl connection (to gazebo_mavlink_interface plugin) -->
<param name="fcu_url" value="udp://$(arg fcu_ip):14540@" if="$(eval fcu_conn == 'hitl')"/>
<!-- set target_system_id -->
<param name="target_system_id" value="$(arg fcu_sys_id)" />
@@ -39,7 +42,7 @@
<rosparam command="load" file="$(find clover)/launch/mavros_config.yaml"/>
<!-- remap rangefinder -->
<remap from="mavros/distance_sensor/rangefinder_sub" to="rangefinder/range"/>
<remap from="mavros/distance_sensor/rangefinder_sub" to="$(arg distance_sensor_remap)" if="$(eval bool(distance_sensor_remap))"/>
<rosparam param="plugin_whitelist">
- altitude

9
clover/launch/mavros_config.yaml
View File

@@ -1,11 +1,11 @@
# Config file for mavros
# Based on https://raw.githubusercontent.com/mavlink/mavros/master/mavros/launch/px4_config.yaml
startup_px4_usb_quirk: true
startup_px4_usb_quirk: false
conn:
heartbeat_rate: 1.0 # send hertbeat rate in Hertz
timeout: 10.0 # hertbeat timeout in seconds
heartbeat_rate: 1.0 # send heartbeat rate in Hertz
timeout: 10.0 # heartbeat timeout in seconds
timesync_rate: 10.0 # TIMESYNC rate in Hertz (feature disabled if 0.0)
system_time_rate: 1.0 # send system time to FCU rate in Hertz (disabled if 0.0)
@@ -78,6 +78,9 @@ distance_sensor:
field_of_view: 0.5
rangefinder_sub:
subscriber: true
id: 1
orientation: PITCH_270
covariance: 1 # cm
# fake_gps
fake_gps:

4
clover/launch/simulator.launch Normal file
View File

@@ -0,0 +1,4 @@
<launch>
<!-- shurtcut for running the simulation (`roslaunch clover simulator.launch`) -->
<include file="$(find clover_simulation)/launch/simulator.launch"/>
</launch>

5
clover/package.xml
View File

@@ -1,7 +1,7 @@
<?xml version="1.0"?>
<package format="3">
<name>clover</name>
<version>0.21.1</version>
<version>0.23.0</version>
<description>The Clover package</description>
<maintainer email="okalachev@gmail.com">Oleg Kalachev</maintainer>
@@ -37,8 +37,11 @@
<depend>rosbridge_server</depend>
<depend>web_video_server</depend>
<depend>tf2_web_republisher</depend>
<depend>libxml2</depend>
<depend>libxslt</depend>
<depend condition="$ROS_PYTHON_VERSION == 2">python-lxml</depend>
<depend condition="$ROS_PYTHON_VERSION == 3">python3-lxml</depend>
<depend>dynamic_reconfigure</depend>
<exec_depend>python-pymavlink</exec_depend>
<!-- Use test_depend for packages you need only for testing: -->
<!-- <test_depend>gtest</test_depend> -->

11
clover/setup.py Normal file
View File

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

87
clover/src/autotest/autotest_aruco.py Executable file
View File

@@ -0,0 +1,87 @@
#!/usr/bin/env python3
import rospy
import math
import signal
import sys
import dynamic_reconfigure.client
from clover import srv
from std_srvs.srv import Trigger
from sensor_msgs.msg import Range
from aruco_pose.msg import MarkerArray
from util import handle_response
rospy.init_node('autotest_aruco', disable_signals=True) # disable signals to allow interrupting with ctrl+c
flow_client = dynamic_reconfigure.client.Client('optical_flow')
get_telemetry = rospy.ServiceProxy('get_telemetry', srv.GetTelemetry)
navigate = handle_response(rospy.ServiceProxy('navigate', srv.Navigate))
land = handle_response(rospy.ServiceProxy('land', Trigger))
def interrupt(sig, frame):
print('\nInterrupted, landing...')
land()
sys.exit(0)
signal.signal(signal.SIGINT, interrupt)
def print_current_map_position():
telem = get_telemetry()
dist = rospy.wait_for_message('rangefinder/range', Range).range
print('Map position:\tx={:.1f}\ty={:.1f}\tz={:.1f}\tyaw={:.1f}\tdist={:.2f}'.format(telem.x, telem.y, telem.z, telem.yaw, dist))
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)
markers = rospy.wait_for_message('aruco_map/map', MarkerArray, timeout=3)
left = min(marker.pose.position.x for marker in markers.markers)
bottom = min(marker.pose.position.y for marker in markers.markers)
width = max(marker.pose.position.x for marker in markers.markers)
height = max(marker.pose.position.y for marker in markers.markers)
center_x = left + width / 2
center_y = bottom + height / 2
print('Map rect: %g %g - %g %g' % (left, bottom, width, height))
input('Take off and hover 1 m [enter] ')
navigate_wait(x=0, y=0, z=1, frame_id='body', auto_arm=True)
print_current_map_position()
input('Go to corner %g %g 1.5 speed 1 [enter] ' % (width, height))
navigate_wait(x=width, y=height, z=1.5, speed=1, frame_id='aruco_map')
print_current_map_position()
input('Go to center %g %g 1.5 speed 5 [enter] ' % (center_x, center_y))
navigate_wait(x=center_x, y=center_y, z=1.5, speed=5, frame_id='aruco_map')
print_current_map_position()
input('Disable optical flow and keep hovering [enter] ')
flow_client.update_configuration({'enabled': False})
rospy.sleep(5)
input('Enable optical flow back [enter] ')
flow_client.update_configuration({'enabled': True})
input('Go to side 1 %g 2 heading top [enter] ' % (center_y))
navigate_wait(x=1, y=center_y, z=2, yaw=1.57, frame_id='aruco_map')
print_current_map_position()
marker_id = markers.markers[0].id
input('Go to marker %d z=1.5 [enter] ' % marker_id)
navigate_wait(x=0, y=0, z=1.5, yaw=0, frame_id='aruco_%d' % marker_id)
print_current_map_position()
input('Perform landing [enter] ')
land()

100
clover/src/autotest/autotest_flight.py Executable file
View File

@@ -0,0 +1,100 @@
#!/usr/bin/env python3
import rospy
import math
from math import nan
import signal
import sys
from clover import srv
from std_srvs.srv import Trigger
from sensor_msgs.msg import Range
from util import handle_response
rospy.init_node('autotest_flight', disable_signals=True) # disable signals to allow interrupting with ctrl+c
get_telemetry = rospy.ServiceProxy('get_telemetry', srv.GetTelemetry)
navigate = handle_response(rospy.ServiceProxy('navigate', srv.Navigate))
navigate_global = handle_response(rospy.ServiceProxy('navigate_global', srv.NavigateGlobal))
set_position = handle_response(rospy.ServiceProxy('set_position', srv.SetPosition))
set_velocity = handle_response(rospy.ServiceProxy('set_velocity', srv.SetVelocity))
set_attitude = handle_response(rospy.ServiceProxy('set_attitude', srv.SetAttitude))
set_rates = handle_response(rospy.ServiceProxy('set_rates', srv.SetRates))
land = handle_response(rospy.ServiceProxy('land', Trigger))
def interrupt(sig, frame):
print('\nInterrupted, landing...')
land()
sys.exit(0)
signal.signal(signal.SIGINT, interrupt)
def navigate_wait(x=0, y=0, z=0, yaw=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)
def print_distance():
dist = rospy.wait_for_message('rangefinder/range', Range).range
print('Distance: {:.2f}'.format(dist))
input('Take off and hover 1 m [enter] ')
navigate_wait(z=1, frame_id='body', auto_arm=True)
print_distance()
start = get_telemetry()
input('Fly forward 2 m [enter] ')
navigate_wait(x=2, frame_id='navigate_target')
print_distance()
input('Climb 0.5 m [enter] ')
navigate_wait(z=0.5, frame_id='navigate_target')
print_distance()
input('Rotate left 90° [enter] ')
navigate(yaw=math.pi / 2, frame_id='navigate_target')
rospy.sleep(3)
input('Use set_velocity to fly forward 2 m speed 1 [enter]')
set_velocity(vx=1, vy=0.0, vz=0, frame_id='body')
rospy.sleep(2)
set_position(frame_id='body')
input('Rotate right 90° [enter] ')
navigate(yaw=-math.pi / 2, frame_id='navigate_target')
rospy.sleep(3)
input('Use set_attitude to fly backwards [enter]')
set_attitude(pitch=-0.3, roll=0, yaw=0, thrust=0.5, frame_id='body')
rospy.sleep(0.3)
set_position(frame_id='body')
input('Use set_attitude to fly right [enter]')
set_attitude(pitch=0, roll=0.3, yaw=0, thrust=0.5, frame_id='body')
rospy.sleep(0.5)
set_position(frame_id='body')
input('Use set_rates to fly right [enter]')
set_rates(roll_rate=1.2, thrust=0.5)
rospy.sleep(0.4)
set_position(frame_id='body')
input('Rotate 360° to the right using yaw_rate [enter]')
set_position(x=nan, y=nan, z=nan, frame_id='body', yaw=nan, yaw_rate=-1)
rospy.sleep(2 * math.pi)
set_position(frame_id='body')
input('Return to start point [enter]')
navigate_wait(x=start.x, y=start.y, z=start.z, yaw=start.yaw, speed=1, frame_id='map')
input('Land [enter]')
land()

72
clover/src/autotest/autotest_led.py Executable file
View File

@@ -0,0 +1,72 @@
#!/usr/bin/env python3
import rospy
import functools
from clover.srv import SetLEDEffect
from led_msgs.srv import SetLEDs
from led_msgs.msg import LEDStateArray, LEDState
from util import handle_response
rospy.init_node('autotest_led', disable_signals=True)
set_leds = handle_response(rospy.ServiceProxy('led/set_leds', SetLEDs))
set_effect = handle_response(rospy.ServiceProxy('led/set_effect', SetLEDEffect))
led_count = len(rospy.wait_for_message('led/state', LEDStateArray, timeout=10).leds)
print('LED count =', led_count)
print('== Testing effects ==')
input('Fill red [enter] ')
set_effect(r=255, g=0, b=0)
input('Fill green [enter] ')
set_effect(r=0, g=100, b=0)
input('Blink white [enter] ')
set_effect(effect='blink', r=255, g=255, b=255)
rospy.sleep(3)
input('Blink fast violet [enter] ')
set_effect(effect='blink_fast', r=220, g=20, b=250)
rospy.sleep(3)
input('Fade to blue [enter] ')
set_effect(effect='fade', r=0, g=0, b=255)
input('Wipe to yellow [enter] ')
set_effect(effect='wipe', r=255, g=255, b=40)
input('Flash red [enter] ')
set_effect(effect='flash', r=255, g=0, b=0)
rospy.sleep(1)
input('Rainbow [enter] ')
set_effect(effect='rainbow')
rospy.sleep(4)
input('Rainbow fill [enter] ')
set_effect(effect='rainbow_fill')
rospy.sleep(4)
input('Turn off [enter] ')
set_effect()
print('== Testing low-level control ==')
input('Fill orange [enter] ')
set_leds(leds=[LEDState(index=i, r=245, g=155, b=0) for i in range(led_count)])
input('Fill blue gradient [enter] ')
set_leds(leds=[LEDState(index=i, r=0, g=20, b=int(255 * i / led_count)) for i in range(led_count)])
input('Animate green dot [enter] ')
set_effect()
for i in range(led_count):
if i > 0:
set_leds(leds=[LEDState(index=i - 1, r=0, g=0, b=0)])
set_leds(leds=[LEDState(index=i, r=0, g=255, b=0)])
rospy.sleep(0.05)
input('Turn off [enter] ')
set_effect()

11
clover/src/autotest/util.py Normal file
View File

@@ -0,0 +1,11 @@
import functools
# decorator to handle response and print error message
def handle_response(fn):
@functools.wraps(fn)
def wrapper(*args, **kwargs):
res = fn(*args, **kwargs)
if not res.success:
print('\033[91mError:\033[0m {}'.format(res.message))
return res
return wrapper

35
clover/src/clover/__init__.py Normal file
View File

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

11
clover/src/led.cpp
View File

@@ -31,7 +31,6 @@ ros::Time start_time;
double blink_rate, blink_fast_rate, flash_delay, fade_period, wipe_period, rainbow_period;
double low_battery_threshold;
std::vector<std::string> error_ignore;
bool blink_state;
led_msgs::SetLEDs set_leds;
led_msgs::LEDStateArray state, start_state;
ros::ServiceClient set_leds_srv;
@@ -87,9 +86,8 @@ void proceed(const ros::TimerEvent& event)
set_leds.request.leds.resize(led_count);
if (current_effect.effect == "blink" || current_effect.effect == "blink_fast") {
blink_state = !blink_state;
// toggle all leds
if (blink_state) {
// enable on odd counter
if (counter % 2 != 0) {
fill(current_effect.r, current_effect.g, current_effect.b);
} else {
fill(0, 0, 0);
@@ -222,6 +220,7 @@ bool setEffect(clover::SetLEDEffect::Request& req, clover::SetLEDEffect::Respons
counter = 0;
start_state = state;
start_time = ros::Time::now();
proceed({ .current_real = start_time });
return true;
}
@@ -320,8 +319,8 @@ int main(int argc, char **argv)
auto set_effect = nh.advertiseService("set_effect", &setEffect);
auto mavros_state_sub = nh.subscribe("/mavros/state", 1, &handleMavrosState);
auto battery_sub = nh.subscribe("/mavros/battery", 1, &handleBattery);
auto mavros_state_sub = nh.subscribe("mavros/state", 1, &handleMavrosState);
auto battery_sub = nh.subscribe("mavros/battery", 1, &handleBattery);
auto rosout_sub = nh.subscribe("/rosout_agg", 1, &handleLog);
timer = nh.createTimer(ros::Duration(0), &proceed, false, false);

57
clover/src/optical_flow.cpp
View File

@@ -22,11 +22,14 @@
#include <tf2/utils.h>
#include <tf2_ros/transform_listener.h>
#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
#include <dynamic_reconfigure/server.h>
#include <mavros_msgs/OpticalFlowRad.h>
#include <sensor_msgs/Imu.h>
#include <geometry_msgs/PoseStamped.h>
#include <geometry_msgs/Vector3Stamped.h>
#include <geometry_msgs/PointStamped.h>
#include <geometry_msgs/TwistStamped.h>
#include <clover/FlowConfig.h>
using cv::Mat;
@@ -38,6 +41,7 @@ public:
{}
private:
bool enabled_;
ros::Publisher flow_pub_, velo_pub_, shift_pub_;
ros::Time prev_stamp_;
std::string fcu_frame_id_, local_frame_id_;
@@ -54,6 +58,10 @@ private:
std::unique_ptr<tf2_ros::TransformListener> tf_listener_;
bool calc_flow_gyro_;
float flow_gyro_default_;
bool disable_on_vpe_;
ros::Subscriber vpe_sub_;
ros::Time last_vpe_time_;
std::shared_ptr<dynamic_reconfigure::Server<clover::FlowConfig>> dyn_srv_;
void onInit()
{
@@ -83,6 +91,17 @@ private:
img_sub_ = it.subscribeCamera("image_raw", 1, &OpticalFlow::flow, this);
disable_on_vpe_ = nh_priv.param("disable_on_vpe", false);
if (disable_on_vpe_) {
vpe_sub_ = nh.subscribe("mavros/vision_pose/pose", 1, &OpticalFlow::vpeCallback, this);
}
dyn_srv_ = std::make_shared<dynamic_reconfigure::Server<clover::FlowConfig>>(nh_priv);
dynamic_reconfigure::Server<clover::FlowConfig>::CallbackType cb;
cb = std::bind(&OpticalFlow::paramCallback, this, std::placeholders::_1, std::placeholders::_2);
dyn_srv_->setCallback(cb);
NODELET_INFO("Optical Flow initialized");
}
@@ -109,6 +128,14 @@ private:
void flow(const sensor_msgs::ImageConstPtr& msg, const sensor_msgs::CameraInfoConstPtr& cinfo)
{
if (!enabled_) return;
if (disable_on_vpe_ &&
!last_vpe_time_.isZero() &&
(msg->header.stamp - last_vpe_time_).toSec() < 0.1) {
return;
}
parseCameraInfo(cinfo);
auto img = cv_bridge::toCvShare(msg, "mono8")->image;
@@ -142,7 +169,7 @@ private:
img.convertTo(curr_, CV_32F);
if (prev_.empty()) {
if (prev_.empty() || (msg->header.stamp - prev_stamp_).toSec() > 0.1) { // outdated previous frame
prev_ = curr_.clone();
prev_stamp_ = msg->header.stamp;
cv::createHanningWindow(hann_, curr_.size(), CV_32F);
@@ -224,6 +251,14 @@ private:
prev_ = curr_.clone();
prev_stamp_ = msg->header.stamp;
// Publish estimated angular velocity
geometry_msgs::TwistStamped velo;
velo.header.stamp = msg->header.stamp;
velo.header.frame_id = fcu_frame_id_;
velo.twist.angular.x = flow_fcu.vector.x / integration_time.toSec();
velo.twist.angular.y = flow_fcu.vector.y / integration_time.toSec();
velo_pub_.publish(velo);
publish_debug:
// Publish debug image
if (img_pub_.getNumSubscribers() > 0) {
@@ -236,14 +271,6 @@ publish_debug:
out_msg.image = img;
img_pub_.publish(out_msg.toImageMsg());
}
// Publish estimated angular velocity
geometry_msgs::TwistStamped velo;
velo.header.stamp = msg->header.stamp;
velo.header.frame_id = fcu_frame_id_;
velo.twist.angular.x = flow_fcu.vector.x / integration_time.toSec();
velo.twist.angular.y = flow_fcu.vector.y / integration_time.toSec();
velo_pub_.publish(velo);
}
}
@@ -264,6 +291,18 @@ publish_debug:
return flow;
}
void paramCallback(clover::FlowConfig &config, uint32_t level)
{
enabled_ = config.enabled;
if (!enabled_) {
prev_ = Mat(); // clear previous frame
}
}
void vpeCallback(const geometry_msgs::PoseStamped& vpe) {
last_vpe_time_ = vpe.header.stamp;
}
};
PLUGINLIB_EXPORT_CLASS(OpticalFlow, nodelet::Nodelet)

2
clover/src/rc.cpp
View File

@@ -91,7 +91,7 @@ private:
void fakeGCSThread()
{
// Awful workaround for fixing PX4 not sending STATUSTEXTs
// if there is no GCS hearbeats.
// if there is no GCS heartbeats.
// TODO: use timer
// TODO: remove, when PX4 get this fixed.
ros::Publisher mavlink_pub = nh.advertise<mavros_msgs::Mavlink>("mavlink/to", 1);

354
clover/src/selfcheck.py
View File

@@ -15,7 +15,8 @@ import subprocess
import re
from collections import OrderedDict
import traceback
from threading import Event
import threading
from threading import Event, Thread, Lock
import numpy
import rospy
import tf2_ros
@@ -27,69 +28,76 @@ from mavros_msgs.msg import State, OpticalFlowRad, Mavlink
from mavros_msgs.srv import ParamGet
from geometry_msgs.msg import PoseStamped, TwistStamped, PoseWithCovarianceStamped, Vector3Stamped
from visualization_msgs.msg import MarkerArray as VisualizationMarkerArray
from diagnostic_msgs.msg import DiagnosticArray
import tf.transformations as t
from aruco_pose.msg import MarkerArray
from mavros import mavlink
# TODO: check attitude is present
# TODO: disk free space
# TODO: map, base_link, body
# TODO: rc service
# TODO: perform commander check, ekf2 status on PX4
# TODO: check if FCU params setter succeed
# TODO: selfcheck ROS service (with blacklists for checks)
import locale
rospy.init_node('selfcheck')
os.environ['ROSCONSOLE_FORMAT']='[${severity}]: ${message}'
os.environ['ROSCONSOLE_FORMAT']='${message}'
# use user's locale to convert numbers, etc
locale.setlocale(locale.LC_ALL, '')
tf_buffer = tf2_ros.Buffer()
tf_listener = tf2_ros.TransformListener(tf_buffer)
failures = []
infos = []
current_check = None
thread_local = threading.local()
reports_lock = Lock()
def failure(text, *args):
msg = text % args
rospy.logwarn('%s: %s', current_check, msg)
failures.append(msg)
thread_local.reports += [{'failure': msg}]
def info(text, *args):
msg = text % args
rospy.loginfo('%s: %s', current_check, msg)
infos.append(msg)
thread_local.reports += [{'info': msg}]
def check(name):
def inner(fn):
def wrapper(*args, **kwargs):
failures[:] = []
infos[:] = []
global current_check
current_check = name
start = rospy.get_time()
thread_local.reports = []
try:
fn(*args, **kwargs)
except Exception as e:
traceback.print_exc()
rospy.logerr('%s: exception occurred', name)
return
if not failures and not infos:
rospy.loginfo('%s: OK', name)
with reports_lock:
for report in thread_local.reports:
if 'failure' in report:
rospy.logerr('%s: %s', name, report['failure'])
elif 'info' in report:
rospy.loginfo('\033[90m%s\033[0m: %s', name, report['info'])
if not thread_local.reports:
rospy.loginfo('\033[90m%s\033[0m: \033[92mOK\033[0m', name)
if rospy.get_param('~time', False):
rospy.loginfo('%s: %.1f sec', name, rospy.get_time() - start)
return wrapper
return inner
def ff(value, precision=2):
# safely format float or int
if value is None:
return '\033[31m???\033[0m'
if isinstance(value, float):
return ('{:.' + str(precision + 1) + '}').format(value)
elif isinstance(value, int):
return str(value)
param_get = rospy.ServiceProxy('mavros/param/get', ParamGet)
def get_param(name):
def get_param(name, default=None):
try:
res = param_get(param_id=name)
except rospy.ServiceException as e:
@@ -98,12 +106,17 @@ def get_param(name):
if not res.success:
failure('unable to retrieve PX4 parameter %s', name)
return default
else:
if res.value.integer != 0:
return res.value.integer
return res.value.real
def get_paramf(name, precision=2):
return ff(get_param(name), precision)
recv_event = Event()
link = mavutil.mavlink.MAVLink('', 255, 1)
mavlink_pub = rospy.Publisher('mavlink/to', Mavlink, queue_size=1)
@@ -148,6 +161,24 @@ def mavlink_exec(cmd, timeout=3.0):
return mavlink_recv
def read_diagnostics(name, key):
e = Event()
def cb(msg):
for status in msg.status:
if status.name.lower() == name.lower():
for value in status.values:
if value.key.lower() == key.lower():
cb.value = value.value
e.set()
return
cb.value = None
sub = rospy.Subscriber('/diagnostics', DiagnosticArray, cb)
e.wait(1.0) # wait to read all the diagnostics from nodes publishing them
sub.unregister()
return cb.value
BOARD_ROTATIONS = {
0: 'no rotation',
1: 'yaw 45°',
@@ -195,24 +226,28 @@ def check_fcu():
failure('no connection to the FCU (check wiring)')
return
# Make sure the console is available to us
mavlink_exec('\n')
version_str = mavlink_exec('ver all')
if version_str == '':
info('no version data available from SITL')
if not is_process_running('px4', exact=True): # can't use px4 console in SITL
clover_tag = re.compile(r'-cl[oe]ver\.\d+$')
clover_fw = False
r = re.compile(r'^FW (git tag|version): (v?\d\.\d\.\d.*)$')
is_clover_firmware = False
for ver_line in version_str.split('\n'):
match = r.search(ver_line)
if match is not None:
field, version = match.groups()
info('firmware %s: %s' % (field, version))
if 'clover' in version or 'clever' in version:
is_clover_firmware = True
# Make sure the console is available to us
mavlink_exec('\n')
version_str = mavlink_exec('ver all')
if version_str == '':
info('no version data available from SITL')
if not is_clover_firmware:
failure('not running Clover PX4 firmware, https://clover.coex.tech/firmware')
for line in version_str.split('\n'):
if line.startswith('FW version: '):
info(line[len('FW version: '):])
elif line.startswith('FW git tag: '): # only Clover's firmware
tag = line[len('FW git tag: '):]
clover_fw = clover_tag.search(tag)
info(tag)
elif line.startswith('HW arch: '):
info(line[len('HW arch: '):])
if not clover_fw:
info('not Clover PX4 firmware, check https://clover.coex.tech/firmware')
est = get_param('SYS_MC_EST_GROUP')
if est == 1:
@@ -249,18 +284,25 @@ def check_fcu():
if cbrk_usb_chk != 197848:
failure('set parameter CBRK_USB_CHK to 197848 for flying with USB connected')
if not is_process_running('px4', exact=True): # skip battery check in SITL
try:
battery = rospy.wait_for_message('mavros/battery', BatteryState, timeout=3)
if not battery.cell_voltage:
failure('cell voltage is not available, https://clover.coex.tech/power')
else:
cell = battery.cell_voltage[0]
if cell > 4.3 or cell < 3.0:
failure('incorrect cell voltage: %.2f V, https://clover.coex.tech/power', cell)
elif cell < 3.7:
failure('critically low cell voltage: %.2f V, recharge battery', cell)
except rospy.ROSException:
failure('no battery state')
# time sync check
try:
battery = rospy.wait_for_message('mavros/battery', BatteryState, timeout=3)
if not battery.cell_voltage:
failure('cell voltage is not available, https://clover.coex.tech/power')
else:
cell = battery.cell_voltage[0]
if cell > 4.3 or cell < 3.0:
failure('incorrect cell voltage: %.2f V, https://clover.coex.tech/power', cell)
elif cell < 3.7:
failure('critically low cell voltage: %.2f V, recharge battery', cell)
except rospy.ROSException:
failure('no battery state')
info('time sync offset: %.2f s', float(read_diagnostics('mavros: Time Sync', 'Estimated time offset (s)')))
except:
failure('cannot read time sync offset')
except rospy.ROSException:
failure('no MAVROS state (check wiring)')
@@ -330,7 +372,7 @@ def is_process_running(binary, exact=False, full=False):
if exact:
args.append('-x') # match exactly with the command name
if full:
args.append('-f') # use full process name to match
args.append('-f') # use full command line (including arguments) to match
args.append(binary)
subprocess.check_output(args)
return True
@@ -340,6 +382,8 @@ def is_process_running(binary, exact=False, full=False):
@check('ArUco markers')
def check_aruco():
markers = None
if is_process_running('aruco_detect', full=True):
try:
info('aruco_detect/length = %g m', rospy.get_param('aruco_detect/length'))
@@ -350,9 +394,9 @@ def check_aruco():
known_tilt += ' (ALL markers are on the floor)'
elif known_tilt == 'map_flipped':
known_tilt += ' (ALL markers are on the ceiling)'
info('aruco_detector/known_tilt = %s', known_tilt)
info('aruco_detect/known_tilt = %s', known_tilt)
try:
rospy.wait_for_message('aruco_detect/markers', MarkerArray, timeout=1)
markers = rospy.wait_for_message('aruco_detect/markers', MarkerArray, timeout=0.8)
except rospy.ROSException:
failure('no markers detection')
return
@@ -369,34 +413,49 @@ def check_aruco():
info('aruco_map/known_tilt = %s', known_tilt)
try:
visualization = rospy.wait_for_message('aruco_map/visualization', VisualizationMarkerArray, timeout=1)
visualization = rospy.wait_for_message('aruco_map/visualization', VisualizationMarkerArray, timeout=0.8)
info('map has %s markers', len(visualization.markers))
except:
failure('cannot read aruco_map/visualization topic')
try:
rospy.wait_for_message('aruco_map/pose', PoseWithCovarianceStamped, timeout=1)
rospy.wait_for_message('aruco_map/pose', PoseWithCovarianceStamped, timeout=0.8)
except rospy.ROSException:
failure('no map detection')
if not markers:
info('no map detection as no markers detection')
elif not markers.markers:
info('no map detection as no markers detected')
else:
failure('no map detection')
else:
info('aruco_map is not running')
def is_on_the_floor():
try:
dist = rospy.wait_for_message('rangefinder/range', Range, timeout=1)
return dist.range < 0.3
except rospy.ROSException:
return False
@check('Vision position estimate')
def check_vpe():
vis = None
try:
vis = rospy.wait_for_message('mavros/vision_pose/pose', PoseStamped, timeout=1)
vis = rospy.wait_for_message('mavros/vision_pose/pose', PoseStamped, timeout=0.8)
except rospy.ROSException:
try:
vis = rospy.wait_for_message('mavros/mocap/pose', PoseStamped, timeout=1)
vis = rospy.wait_for_message('mavros/mocap/pose', PoseStamped, timeout=0.8)
except rospy.ROSException:
failure('no VPE or MoCap messages')
# check if vpe_publisher is running
try:
subprocess.check_output(['pgrep', '-x', 'vpe_publisher'])
except subprocess.CalledProcessError:
return # it's not running, skip following checks
if not is_process_running('vpe_publisher', full=True):
info('no vision position estimate, vpe_publisher is not running')
elif rospy.get_param('aruco_map/known_tilt') == 'map_flipped':
failure('no vision position estimate, markers are on the ceiling')
elif is_on_the_floor():
info('no vision position estimate, the drone is on the floor')
else:
failure('no vision position estimate')
# check PX4 settings
est = get_param('SYS_MC_EST_GROUP')
@@ -408,14 +467,14 @@ def check_vpe():
if vision_yaw_w == 0:
failure('vision yaw weight is zero, change ATT_W_EXT_HDG parameter')
else:
info('Vision yaw weight: %.2f', vision_yaw_w)
info('vision yaw weight: %s', ff(vision_yaw_w))
fuse = get_param('LPE_FUSION')
if not fuse & (1 << 2):
failure('vision position fusion is disabled, change LPE_FUSION parameter')
delay = get_param('LPE_VIS_DELAY')
if delay != 0:
failure('LPE_VIS_DELAY parameter is %s, but it should be zero', delay)
info('LPE_VIS_XY is %.2f m, LPE_VIS_Z is %.2f m', get_param('LPE_VIS_XY'), get_param('LPE_VIS_Z'))
failure('LPE_VIS_DELAY = %s, but it should be zero', delay)
info('LPE_VIS_XY = %s m, LPE_VIS_Z = %s m', get_paramf('LPE_VIS_XY'), get_paramf('LPE_VIS_Z'))
elif est == 2:
fuse = get_param('EKF2_AID_MASK')
if not fuse & (1 << 3):
@@ -424,10 +483,10 @@ def check_vpe():
failure('vision yaw fusion is disabled, change EKF2_AID_MASK parameter')
delay = get_param('EKF2_EV_DELAY')
if delay != 0:
failure('EKF2_EV_DELAY is %.2f, but it should be zero', delay)
info('EKF2_EVA_NOISE is %.3f, EKF2_EVP_NOISE is %.3f',
get_param('EKF2_EVA_NOISE'),
get_param('EKF2_EVP_NOISE'))
failure('EKF2_EV_DELAY = %.2f, but it should be zero', delay)
info('EKF2_EVA_NOISE = %s, EKF2_EVP_NOISE = %s',
get_paramf('EKF2_EVA_NOISE', 3),
get_paramf('EKF2_EVP_NOISE', 3))
if not vis:
return
@@ -485,6 +544,12 @@ def check_local_position():
failure('roll is %.2f deg; place copter horizontally or redo level horizon calib',
math.degrees(roll))
if not tf_buffer.can_transform('base_link', pose.header.frame_id, rospy.get_rostime(), rospy.Duration(0.5)):
failure('can\'t transform from %s to base_link (timeout 0.5 s): is TF enabled?', pose.header.frame_id)
if not tf_buffer.can_transform('body', pose.header.frame_id, rospy.get_rostime(), rospy.Duration(0.5)):
failure('can\'t transform from %s to body (timeout 0.5 s)', pose.header.frame_id)
except rospy.ROSException:
failure('no local position')
@@ -519,9 +584,11 @@ def check_velocity():
@check('Global position (GPS)')
def check_global_position():
try:
rospy.wait_for_message('mavros/global_position/global', NavSatFix, timeout=1)
rospy.wait_for_message('mavros/global_position/global', NavSatFix, timeout=0.8)
except rospy.ROSException:
info('no global position')
if get_param('SYS_MC_EST_GROUP') == 2 and (get_param('EKF2_AID_MASK', 0) & (1 << 0)):
failure('enabled GPS fusion may suppress vision position aiding')
@check('Optical flow')
@@ -533,7 +600,7 @@ def check_optical_flow():
# check PX4 settings
rot = get_param('SENS_FLOW_ROT')
if rot != 0:
failure('SENS_FLOW_ROT parameter is %s, but it should be zero', rot)
failure('SENS_FLOW_ROT = %s, but it should be zero', rot)
est = get_param('SYS_MC_EST_GROUP')
if est == 1:
fuse = get_param('LPE_FUSION')
@@ -541,32 +608,36 @@ def check_optical_flow():
failure('optical flow fusion is disabled, change LPE_FUSION parameter')
if not fuse & (1 << 1):
failure('flow gyro compensation is disabled, change LPE_FUSION parameter')
scale = get_param('LPE_FLW_SCALE')
scale = get_param('LPE_FLW_SCALE', 1)
if not numpy.isclose(scale, 1.0):
failure('LPE_FLW_SCALE parameter is %.2f, but it should be 1.0', scale)
failure('LPE_FLW_SCALE = %.2f, but it should be 1.0', scale)
info('LPE_FLW_QMIN is %s, LPE_FLW_R is %.4f, LPE_FLW_RR is %.4f, SENS_FLOW_MINHGT is %.3f, SENS_FLOW_MAXHGT is %.3f',
get_param('LPE_FLW_QMIN'),
get_param('LPE_FLW_R'),
get_param('LPE_FLW_RR'),
get_param('SENS_FLOW_MINHGT'),
get_param('SENS_FLOW_MAXHGT'))
info('LPE_FLW_QMIN = %s, LPE_FLW_R = %s, LPE_FLW_RR = %s',
get_paramf('LPE_FLW_QMIN'),
get_paramf('LPE_FLW_R', 4),
get_paramf('LPE_FLW_RR', 4))
elif est == 2:
fuse = get_param('EKF2_AID_MASK')
fuse = get_param('EKF2_AID_MASK', 0)
if not fuse & (1 << 1):
failure('optical flow fusion is disabled, change EKF2_AID_MASK parameter')
delay = get_param('EKF2_OF_DELAY')
delay = get_param('EKF2_OF_DELAY', 0)
if delay != 0:
failure('EKF2_OF_DELAY is %.2f, but it should be zero', delay)
info('EKF2_OF_QMIN is %s, EKF2_OF_N_MIN is %.4f, EKF2_OF_N_MAX is %.4f, SENS_FLOW_MINHGT is %.3f, SENS_FLOW_MAXHGT is %.3f',
get_param('EKF2_OF_QMIN'),
get_param('EKF2_OF_N_MIN'),
get_param('EKF2_OF_N_MAX'),
get_param('SENS_FLOW_MINHGT'),
get_param('SENS_FLOW_MAXHGT'))
failure('EKF2_OF_DELAY = %.2f, but it should be zero', delay)
info('EKF2_OF_QMIN = %s, EKF2_OF_N_MIN = %s, EKF2_OF_N_MAX = %s',
get_paramf('EKF2_OF_QMIN'),
get_paramf('EKF2_OF_N_MIN', 4),
get_paramf('EKF2_OF_N_MAX', 4))
info('SENS_FLOW_MINHGT = %s, SENS_FLOW_MAXHGT = %s', get_paramf('SENS_FLOW_MINHGT', 3), get_paramf('SENS_FLOW_MAXHGT', 3))
except rospy.ROSException:
failure('no optical flow data (from Raspberry)')
if rospy.get_param('optical_flow/disable_on_vpe', False):
try:
rospy.wait_for_message('mavros/vision_pose/pose', PoseStamped, timeout=1)
info('no optical flow as disable_on_vpe is true')
except:
failure('no optical flow on RPi, disable_on_vpe is true, but no vision pose also')
else:
failure('no optical flow on RPi')
@check('Rangefinder')
@@ -590,7 +661,7 @@ def check_rangefinder():
est = get_param('SYS_MC_EST_GROUP')
if est == 1:
fuse = get_param('LPE_FUSION')
fuse = get_param('LPE_FUSION', 0)
if not fuse & (1 << 5):
info('"pub agl as lpos down" in LPE_FUSION is disabled, NOT operating over flat surface')
else:
@@ -611,16 +682,20 @@ def check_rangefinder():
@check('Boot duration')
def check_boot_duration():
if not os.path.exists('/etc/clover_version'):
info('skip check')
return # Don't check not on Clover's image
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:
if duration > 20:
failure('long Raspbian boot duration: %ss (systemd-analyze for analyzing)', duration)
@check('CPU usage')
def check_cpu_usage():
WHITELIST = 'nodelet', 'gzclient', 'gzserver'
WHITELIST = 'nodelet', 'gzclient', 'gzserver', 'selfcheck.py'
CMD = "top -n 1 -b -i | tail -n +8 | awk '{ printf(\"%-8s\\t%-8s\\t%-8s\\n\", $1, $9, $12); }'"
output = subprocess.check_output(CMD, shell=True).decode()
processes = output.split('\n')
@@ -629,13 +704,16 @@ def check_cpu_usage():
continue
pid, cpu, cmd = process.split('\t')
if cmd.strip() not in WHITELIST and float(cpu) > 30:
if cmd.strip() not in WHITELIST and locale.atof(cpu) > 30:
failure('high CPU usage (%s%%) detected: %s (PID %s)',
cpu.strip(), cmd.strip(), pid.strip())
@check('clover.service')
def check_clover_service():
if not os.path.exists('/etc/clover_version'):
return # Don't check not on Clover's image
try:
output = subprocess.check_output('systemctl show -p ActiveState --value clover.service'.split(),
stderr=subprocess.STDOUT).decode()
@@ -686,11 +764,18 @@ def check_image():
try:
info('version: %s', open('/etc/clover_version').read().strip())
except IOError:
info('no /etc/clover_version file, not the Clover image?')
try:
info('VM version: %s', open('/etc/clover_vm_version').read().strip())
except IOError:
info('no /etc/clover_version file, not the Clover image?')
@check('Preflight status')
def check_preflight_status():
if is_process_running('px4', exact=True):
info('can\'t check in SITL')
return
# Make sure the console is available to us
mavlink_exec('\n')
cmdr_output = mavlink_exec('commander check')
@@ -712,6 +797,10 @@ def check_preflight_status():
@check('Network')
def check_network():
if not os.path.exists('/etc/clover_version'):
# TODO:
return # Don't check not on Clover's image
ros_hostname = os.environ.get('ROS_HOSTNAME', '').strip()
if not ros_hostname:
@@ -734,6 +823,14 @@ def check_network():
@check('RPi health')
def check_rpi_health():
try:
import shutil
total, used, free = shutil.disk_usage('/')
if free < 1024 * 1024 * 1024:
failure('disk space is less than 1 GB; consider removing logs (~/.ros/log/)')
except Exception as e:
info('could not check the disk free space: %s', str(e))
# `vcgencmd get_throttled` output codes taken from
# https://github.com/raspberrypi/documentation/blob/JamesH65-patch-vcgencmd-vcdbg-docs/raspbian/applications/vcgencmd.md#get_throttled
# TODO: support more base platforms?
@@ -781,26 +878,47 @@ def check_board():
info('could not open /proc/device-tree/model, not a Raspberry Pi?')
def parallel_for(fns):
threads = []
for fn in fns:
thread = Thread(target=fn)
thread.start()
threads.append(thread)
for thread in threads:
thread.join()
def consequentially_for(fns):
for fn in fns:
fn()
def selfcheck():
check_image()
check_board()
check_clover_service()
check_network()
check_fcu()
check_imu()
check_local_position()
check_velocity()
check_global_position()
check_preflight_status()
check_main_camera()
check_aruco()
check_simpleoffboard()
check_optical_flow()
check_vpe()
check_rangefinder()
check_rpi_health()
check_cpu_usage()
check_boot_duration()
checks = [
check_image,
check_board,
check_clover_service,
check_network,
check_fcu,
check_imu,
check_local_position,
check_velocity,
check_global_position,
check_preflight_status,
check_main_camera,
check_aruco,
check_simpleoffboard,
check_optical_flow,
check_vpe,
check_rangefinder,
check_rpi_health,
check_cpu_usage,
check_boot_duration,
]
if rospy.get_param('~parallel', False):
parallel_for(checks)
else:
consequentially_for(checks)
if __name__ == '__main__':

63
clover/src/simple_offboard.cpp
View File

@@ -61,6 +61,7 @@ std::shared_ptr<tf2_ros::TransformBroadcaster> transform_broadcaster;
std::shared_ptr<tf2_ros::StaticTransformBroadcaster> static_transform_broadcaster;
// Parameters
string mavros;
string local_frame;
string fcu_frame;
ros::Duration transform_timeout;
@@ -149,6 +150,9 @@ void handleState(const mavros_msgs::State& s)
inline void publishBodyFrame()
{
if (body.child_frame_id.empty()) return;
if (!body.header.stamp.isZero() && body.header.stamp == local_position.header.stamp) {
return; // avoid TF_REPEATED_DATA warnings
}
tf::Quaternion q;
q.setRPY(0, 0, tf::getYaw(local_position.pose.orientation));
@@ -690,7 +694,7 @@ 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
// destination point and/or attitude
PoseStamped ps;
ps.header.frame_id = frame_id;
ps.header.stamp = stamp;
@@ -699,7 +703,12 @@ bool serve(enum setpoint_type_t sp_type, float x, float y, float z, float vx, fl
ps.pose.position.z = z;
ps.pose.orientation.w = 1.0; // Ensure quaternion is always valid
if (std::isnan(yaw)) {
if (sp_type == ATTITUDE) {
ps.pose.position.x = 0;
ps.pose.position.y = 0;
ps.pose.position.z = 0;
ps.pose.orientation = tf::createQuaternionMsgFromRollPitchYaw(roll, pitch, yaw);
} else if (std::isnan(yaw)) {
setpoint_yaw_type = YAW_RATE;
setpoint_yaw_rate = yaw_rate;
} else if (std::isinf(yaw) && yaw > 0) {
@@ -851,6 +860,13 @@ bool land(std_srvs::Trigger::Request& req, std_srvs::Trigger::Response& res)
return false;
}
bool release(std_srvs::Trigger::Request& req, std_srvs::Trigger::Response& res)
{
setpoint_timer.stop();
res.success = true;
return true;
}
int main(int argc, char **argv)
{
ros::init(argc, argv, "simple_offboard");
@@ -861,8 +877,9 @@ int main(int argc, char **argv)
static_transform_broadcaster = std::make_shared<tf2_ros::StaticTransformBroadcaster>();
// Params
nh.param<string>("mavros/local_position/tf/frame_id", local_frame, "map");
nh.param<string>("mavros/local_position/tf/child_frame_id", fcu_frame, "base_link");
nh_priv.param("mavros", mavros, string("mavros")); // for case of using multiple connections
nh.param<string>(mavros + "/local_position/tf/frame_id", local_frame, "map");
nh.param<string>(mavros + "/local_position/tf/child_frame_id", fcu_frame, "base_link");
nh_priv.param("target_frame", target.child_frame_id, string("navigate_target"));
nh_priv.param("setpoint", setpoint.child_frame_id, string("setpoint"));
nh_priv.param("auto_release", auto_release, true);
@@ -873,6 +890,13 @@ int main(int argc, char **argv)
nh_priv.param<string>("body_frame", body.child_frame_id, "body");
nh_priv.getParam("reference_frames", reference_frames);
// Default reference frames
std::map<string, string> default_reference_frames;
default_reference_frames[body.child_frame_id] = local_frame;
default_reference_frames[fcu_frame] = local_frame;
if (!target.child_frame_id.empty()) default_reference_frames[target.child_frame_id] = local_frame;
reference_frames.insert(default_reference_frames.begin(), default_reference_frames.end()); // merge defaults
state_timeout = ros::Duration(nh_priv.param("state_timeout", 3.0));
local_position_timeout = ros::Duration(nh_priv.param("local_position_timeout", 2.0));
velocity_timeout = ros::Duration(nh_priv.param("velocity_timeout", 2.0));
@@ -887,25 +911,25 @@ int main(int argc, char **argv)
arming_timeout = ros::Duration(nh_priv.param("arming_timeout", 4.0));
// Service clients
arming = nh.serviceClient<mavros_msgs::CommandBool>("mavros/cmd/arming");
set_mode = nh.serviceClient<mavros_msgs::SetMode>("mavros/set_mode");
arming = nh.serviceClient<mavros_msgs::CommandBool>(mavros + "/cmd/arming");
set_mode = nh.serviceClient<mavros_msgs::SetMode>(mavros + "/set_mode");
// Telemetry subscribers
auto state_sub = nh.subscribe("mavros/state", 1, &handleState);
auto velocity_sub = nh.subscribe("mavros/local_position/velocity_body", 1, &handleMessage<TwistStamped, velocity>);
auto global_position_sub = nh.subscribe("mavros/global_position/global", 1, &handleMessage<NavSatFix, global_position>);
auto battery_sub = nh.subscribe("mavros/battery", 1, &handleMessage<BatteryState, battery>);
auto statustext_sub = nh.subscribe("mavros/statustext/recv", 1, &handleMessage<mavros_msgs::StatusText, statustext>);
auto manual_control_sub = nh.subscribe("mavros/manual_control/control", 1, &handleMessage<mavros_msgs::ManualControl, manual_control>);
auto local_position_sub = nh.subscribe("mavros/local_position/pose", 1, &handleLocalPosition);
auto state_sub = nh.subscribe(mavros + "/state", 1, &handleState);
auto velocity_sub = nh.subscribe(mavros + "/local_position/velocity_body", 1, &handleMessage<TwistStamped, velocity>);
auto global_position_sub = nh.subscribe(mavros + "/global_position/global", 1, &handleMessage<NavSatFix, global_position>);
auto battery_sub = nh.subscribe(mavros + "/battery", 1, &handleMessage<BatteryState, battery>);
auto statustext_sub = nh.subscribe(mavros + "/statustext/recv", 1, &handleMessage<mavros_msgs::StatusText, statustext>);
auto manual_control_sub = nh.subscribe(mavros + "/manual_control/control", 1, &handleMessage<mavros_msgs::ManualControl, manual_control>);
auto local_position_sub = nh.subscribe(mavros + "/local_position/pose", 1, &handleLocalPosition);
// Setpoint publishers
position_pub = nh.advertise<PoseStamped>("mavros/setpoint_position/local", 1);
position_raw_pub = nh.advertise<PositionTarget>("mavros/setpoint_raw/local", 1);
attitude_pub = nh.advertise<PoseStamped>("mavros/setpoint_attitude/attitude", 1);
attitude_raw_pub = nh.advertise<AttitudeTarget>("mavros/setpoint_raw/attitude", 1);
rates_pub = nh.advertise<TwistStamped>("mavros/setpoint_attitude/cmd_vel", 1);
thrust_pub = nh.advertise<Thrust>("mavros/setpoint_attitude/thrust", 1);
position_pub = nh.advertise<PoseStamped>(mavros + "/setpoint_position/local", 1);
position_raw_pub = nh.advertise<PositionTarget>(mavros + "/setpoint_raw/local", 1);
attitude_pub = nh.advertise<PoseStamped>(mavros + "/setpoint_attitude/attitude", 1);
attitude_raw_pub = nh.advertise<AttitudeTarget>(mavros + "/setpoint_raw/attitude", 1);
rates_pub = nh.advertise<TwistStamped>(mavros + "/setpoint_attitude/cmd_vel", 1);
thrust_pub = nh.advertise<Thrust>(mavros + "/setpoint_attitude/thrust", 1);
// Service servers
auto gt_serv = nh.advertiseService("get_telemetry", &getTelemetry);
@@ -916,6 +940,7 @@ int main(int argc, char **argv)
auto sa_serv = nh.advertiseService("set_attitude", &setAttitude);
auto sr_serv = nh.advertiseService("set_rates", &setRates);
auto ld_serv = nh.advertiseService("land", &land);
auto rl_serv = nh_priv.advertiseService("release", &release);
// Setpoint timer
setpoint_timer = nh.createTimer(ros::Duration(1 / nh_priv.param("setpoint_rate", 30.0)), &publishSetpoint, false, false);

16
clover/src/vpe_publisher.cpp
View File

@@ -33,14 +33,14 @@ ros::Subscriber local_position_sub;
ros::Timer zero_timer;
PoseStamped vpe, pose;
ros::Time got_local_pos(0);
ros::Duration publish_zero_timout, publish_zero_duration, offset_timeout;
ros::Duration publish_zero_timeout, publish_zero_duration, offset_timeout;
TransformStamped offset;
void publishZero(const ros::TimerEvent& e)
{
if (e.current_real - vpe.header.stamp < publish_zero_timout) return; // have vpe
if (!vpe.header.stamp.isZero() && e.current_real - vpe.header.stamp < publish_zero_timeout) return; // have vpe
if (e.current_real - pose.header.stamp < publish_zero_timout) { // have local position
if (!pose.header.stamp.isZero() && e.current_real - pose.header.stamp < publish_zero_timeout) { // have local position
if (got_local_pos.isZero()) {
ROS_INFO("got local position");
got_local_pos = e.current_real;
@@ -124,8 +124,8 @@ int main(int argc, char **argv) {
nh_priv.param<string>("frame_id", frame_id, "");
nh_priv.param<string>("offset_frame_id", offset_frame_id, "");
nh_priv.param<string>("mavros/local_position/frame_id", local_frame_id, "map");
nh_priv.param<string>("mavros/local_position/tf/child_frame_id", child_frame_id, "base_link");
nh.param<string>("mavros/local_position/frame_id", local_frame_id, "map");
nh.param<string>("mavros/local_position/tf/child_frame_id", child_frame_id, "base_link");
offset_timeout = ros::Duration(nh_priv.param("offset_timeout", 3.0));
if (!frame_id.empty()) {
@@ -141,11 +141,11 @@ int main(int argc, char **argv) {
vpe_pub = nh_priv.advertise<PoseStamped>("vpe", 1);
//vpe_cov_pub = nh_priv_.advertise<PoseStamped>("pose_cov_pub", 1);
if (nh_priv.param("publish_zero", false)) {
if (nh_priv.param("force_init", false) || nh_priv.param("publish_zero", false)) { // publish_zero is old name
// publish zero to initialize the local position
zero_timer = nh.createTimer(ros::Duration(0.1), &publishZero);
publish_zero_timout = ros::Duration(nh_priv.param("publish_zero_timout", 5.0));
publish_zero_duration = ros::Duration(nh_priv.param("publish_zero_duration", 5.0));
publish_zero_timeout = ros::Duration(nh_priv.param("force_init_timeout", 5.0));
publish_zero_duration = ros::Duration(nh_priv.param("force_init_duration", 5.0));
local_position_sub = nh.subscribe("mavros/local_position/pose", 1, &localPositionCallback);
}

43
clover/test/basic.py
View File

@@ -3,6 +3,7 @@ import rospy
import pytest
from mavros_msgs.msg import State
from clover import srv
import time
@pytest.fixture()
def node():
@@ -24,6 +25,7 @@ def test_simple_offboard_services_available():
rospy.wait_for_service('set_attitude', timeout=5)
rospy.wait_for_service('set_rates', timeout=5)
rospy.wait_for_service('land', timeout=5)
rospy.wait_for_service('simple_offboard/release', timeout=5)
def test_web_video_server(node):
try:
@@ -33,3 +35,44 @@ def test_web_video_server(node):
# Python 3
import urllib.request as urllib
urllib.urlopen("http://localhost:8080").read()
def test_blocks(node):
rospy.wait_for_service('clover_blocks/run', timeout=5)
rospy.wait_for_service('clover_blocks/stop', timeout=5)
rospy.wait_for_service('clover_blocks/load', timeout=5)
rospy.wait_for_service('clover_blocks/store', timeout=5)
from std_msgs.msg import String
from clover_blocks.srv import Run
def wait_print():
try:
wait_print.result = rospy.wait_for_message('clover_blocks/print', String, timeout=5).data
except Exception as e:
wait_print.result = str(e)
import threading
t = threading.Thread(target=wait_print)
t.start()
rospy.sleep(0.1)
run = rospy.ServiceProxy('clover_blocks/run', Run)
assert run(code='print("test")').success == True
t.join()
assert wait_print.result == 'test'
def test_long_callback():
from clover import long_callback
from time import sleep
# very basic test for long_callback
@long_callback
def cb(i):
cb.counter += i
cb.counter = 0
cb(2)
sleep(0.1)
cb(3)
sleep(1)
assert cb.counter == 5

10
clover/test/basic.test
View File

@@ -23,10 +23,7 @@
<node pkg="tf2_ros" type="static_transform_publisher" name="map_flipped_frame" args="0 0 0 3.1415926 3.1415926 0 map map_flipped" required="true"/>
<node name="simple_offboard" pkg="clover" type="simple_offboard" required="true" output="screen">
<param name="reference_frames/body" value="map"/>
<param name="reference_frames/base_link" value="map"/>
</node>
<node name="simple_offboard" pkg="clover" type="simple_offboard" required="true" output="screen"/>
<node name="tf2_web_republisher" pkg="tf2_web_republisher" type="tf2_web_republisher" required="true"/>
@@ -38,6 +35,11 @@
<rosparam param="notify">startup: { r: 255, g: 255, b: 255 }</rosparam>
</node>
<node name="clover_blocks" pkg="clover_blocks" type="clover_blocks" output="screen" required="true"/>
<node pkg="topic_tools" name="main_camera_throttle" type="throttle" ns="main_camera"
args="messages image_raw 5.0 image_raw_throttled" required="true"/>
<param name="test_module" value="$(find clover)/test/basic.py"/>
<test test-name="basic_test" pkg="ros_pytest" type="ros_pytest_runner"/>
</launch>

23
clover/www/console.html Normal file
View File

@@ -0,0 +1,23 @@
<h1>
/var/log/clover.log
<a style="font-size: 0.5em; vertical-align: super; font-weight: normal" href="clover.log" download>download</a>
</h1>
<pre></pre>
<script type="module">
var pre = document.querySelector('pre');
fetch('clover.log?' + Math.random()).then(function(response) { // random to forbid caching
if (response.status == 404) {
pre.innerHTML = '/var/log/clover.log does not exist';
return;
} else if (response.status !== 200) {
pre.innerHTML('Error ' + response.status);
return;
}
response.text().then(function(content) {
pre.innerHTML = content;
});
});
</script>

2
clover/www/index.html
View File

@@ -9,7 +9,7 @@
<li><a href="" id="butterfly">Open web terminal</a> (<code>Butterfly</code>)</li>
<li>View <a href="viz.html">View 3D visualization</a>, <a href="aruco_map.html">3D visualization for markers map</a> (<code>ros3djs</code>)</li>
<li><a href="../clover_blocks/">Blocks programming</a> (<code>Blockly</code>)</li>
<li><a href="clover.log">Clover console</a> (<code>/var/log/clover.log</code>)</li>
<li><a href="console.html">Clover console</a> (<code>/var/log/clover.log</code>)</li>
</ul>
<p>

17
clover/www/js/topics.js
View File

@@ -1,5 +1,6 @@
const url = 'ws://' + location.hostname + ':9090';
const ros = new ROSLIB.Ros({ url: url });
const params = Object.fromEntries(new URLSearchParams(window.location.search).entries());
ros.on('connection', function () {
document.body.classList.add('connected');
@@ -39,6 +40,7 @@ function viewTopicsList() {
let rosdistro;
function viewTopic(topic) {
let counter = 0;
let index = '<a href=topics.html>Topics</a>';
title.innerHTML = `${index}: ${topic}`;
topicMessage.style.display = 'block';
@@ -50,9 +52,20 @@ function viewTopic(topic) {
});
new ROSLIB.Topic({ ros: ros, name: topic }).subscribe(function(msg) {
counter++;
document.title = topic;
if (mouseDown) return;
topicMessage.innerHTML = yamlStringify(msg); // JSON.stringify(msg, null, 4);
if (msg.header && msg.header.stamp) {
if (params.date || params.offset) {
let date = new Date(msg.header.stamp.secs * 1e3 + msg.header.stamp.nsecs * 1e-6);
if (params.date) msg.header.date = date.toISOString();
if (params.offset) msg.header.offset = (new Date() - date) * 1e-3;
}
}
let txt = `<div class=counter>${counter} received</div>${yamlStringify(msg)}`; // JSON.stringify(msg, null, 4);
topicMessage.innerHTML = txt;
});
}
@@ -62,8 +75,6 @@ topicMessage.addEventListener('mousedown', function() { mouseDown = true; });
topicMessage.addEventListener('mouseup', function() { mouseDown = false; });
function init() {
const params = Object.fromEntries(new URLSearchParams(window.location.search).entries());
if (!params.topic) {
viewTopicsList();
} else {

3
clover/www/topics.html
View File

@@ -15,6 +15,7 @@
white-space: pre;
font-family: monospace;
}
.counter { color: #b9b9b9; margin-bottom: 1em; }
#topic-type { font-family: monospace; font-size: 0.5em; vertical-align: super; font-weight: normal; }
.topic { font-family: monospace; }
body.closed { background-color: rgb(207, 207, 207); }
@@ -23,6 +24,6 @@
<body>
<h1>&nbsp;</h1>
<ul id="topics"></ul>
<code id="topic-message" title="Hold mouse button to pause">No messages received</code>
<code id="topic-message">No messages received</code>
</body>
</html>

10
clover_blocks/README.md
View File

@@ -19,7 +19,7 @@ The frontend files are located in [`www`](./www/) subdirectory. The frontend app
### Services
* `~run` ([*clover_blocks/Run*](srv/Run.srv)) – run Blockly-generated program (in Python).
* `~stop` ([*std_srvs/Trigger*](http://docs.ros.org/melodic/api/std_srvs/html/srv/Trigger.html)) – terminate the running program.
* `~stop` ([*std_srvs/Trigger*](http://docs.ros.org/noetic/api/std_srvs/html/srv/Trigger.html)) – terminate the running program.
* `~store` ([*clover_blocks/load*](srv/Store.srv)) – store a user program (to `<package_path>/programs` by default).
* `~load` ([*clover_blocks/load*](srv/Load.srv)) – load all the stored programs.
@@ -45,11 +45,11 @@ http://<hostname>/clover_blocks/?navigate_tolerance=0.5&sleep_time=0.1
#### Published
* `~running` ([*std_msgs/Bool*](http://docs.ros.org/melodic/api/std_msgs/html/msg/Bool.html)) – indicates if the program is currently running.
* `~block` ([*std_msgs/String*](http://docs.ros.org/melodic/api/std_msgs/html/msg/String.html)) – current executing block (maximum topic rate is limited).
* `~error` ([*std_msgs/String*](http://docs.ros.org/melodic/api/std_msgs/html/msg/String.html)) – user program errors and exceptions.
* `~running` ([*std_msgs/Bool*](http://docs.ros.org/noetic/api/std_msgs/html/msg/Bool.html)) – indicates if the program is currently running.
* `~block` ([*std_msgs/String*](http://docs.ros.org/noetic/api/std_msgs/html/msg/String.html)) – current executing block (maximum topic rate is limited).
* `~error` ([*std_msgs/String*](http://docs.ros.org/noetic/api/std_msgs/html/msg/String.html)) – user program errors and exceptions.
* `~prompt` ([*clover_blocks/Prompt*](msg/Prompt.msg)) – user input request (includes random request ID string).
This topic is published from the frontend side:
* `~prompt/<request_id>` ([*std_msgs/String*](http://docs.ros.org/melodic/api/std_msgs/html/msg/String.html)) – user input response.
* `~prompt/<request_id>` ([*std_msgs/String*](http://docs.ros.org/noetic/api/std_msgs/html/msg/String.html)) – user input response.

2
clover_blocks/package.xml
View File

@@ -1,7 +1,7 @@
<?xml version="1.0"?>
<package format="2">
<name>clover_blocks</name>
<version>0.21.1</version>
<version>0.23.0</version>
<description>Blockly programming support for Clover</description>
<maintainer email="okalachev@gmail.com">Oleg Kalachev</maintainer>
<license>MIT</license>

2
clover_description/package.xml
View File

@@ -1,6 +1,6 @@
<package format="2">
<name>clover_description</name>
<version>0.21.1</version>
<version>0.23.0</version>
<description>The clover_description package provides URDF models of the Clover series of quadcopters.</description>
<maintainer email="sfalexrog@gmail.com">Alexey Rogachevskiy</maintainer>

2
clover_description/urdf/clover/clover4_physics.xacro
View File

@@ -35,7 +35,7 @@
<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)" />
<xacro:property name="color" value="DarkGrey" />
<!-- Property Blocks -->
<!-- Clover body inertia -->

6
clover_description/urdf/leds/led_strip.xacro
View File

@@ -64,6 +64,12 @@
<!-- <gazebo>
<static>true</static>
</gazebo> -->
<gazebo>
<plugin name="${name}_ros_controller" filename="libsim_leds_controller.so">
<robotNamespace></robotNamespace>
<ledCount>${led_count}</ledCount>
</plugin>
</gazebo>
</xacro:macro>
</robot>

4
clover_description/urdf/sensors/distance_sensor.urdf.xacro
View File

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

8
clover_simulation/CMakeLists.txt
View File

@@ -37,6 +37,14 @@ target_compile_options(sim_leds PRIVATE -std=c++11)
add_dependencies(sim_leds ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
add_library(sim_leds_controller src/sim_leds_controller.cpp)
target_include_directories(sim_leds_controller PRIVATE ${catkin_INCLUDE_DIRS} ${GAZEBO_INCLUDE_DIRS})
target_link_libraries(sim_leds_controller PRIVATE ${catkin_LIBRARIES} ${GAZEBO_LIBRARIES})
target_compile_options(sim_leds_controller PRIVATE -std=c++11)
add_dependencies(sim_leds_controller ${${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}

2
clover_simulation/README.md
View File

@@ -10,7 +10,7 @@ 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);
* `vehicle` (*string*, default: *clover*) - PX4 vehicle name. Depending on this parameter, different PX4 presets will be loaded.
* `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;

47
clover_simulation/airframes/4500_clover
View File

@@ -7,30 +7,31 @@
. ${R}etc/init.d-posix/airframes/10016_iris # base on iris
param set ATT_W_EXT_HDG 0.5
param set ATT_EXT_HDG_M 1
param set-default ATT_W_EXT_HDG 0.5
param set-default ATT_EXT_HDG_M 1 # 0 = none, 1 = vision, 2 = mocap
param set COM_DISARM_LAND 1.0
param set-default COM_DISARM_LAND 1.0
param set-default COM_RCL_EXCEPT 4 # enable offboard flights without rc
param set LPE_FLW_SCALE 1.0
param set LPE_FLW_R 0.2
param set LPE_FLW_RR 0.0
param set LPE_FLW_QMIN 10
param set LPE_VIS_DELAY 0.0
param set LPE_VIS_Z 0.1
param set LPE_FUSION 86
param set-default LPE_FLW_SCALE 1.0
param set-default LPE_FLW_R 0.2
param set-default LPE_FLW_RR 0.0
param set-default LPE_FLW_QMIN 10
param set-default LPE_VIS_DELAY 0.0
param set-default LPE_VIS_Z 0.1
param set-default LPE_FUSION 86 # flow + vis + land detector + gyro comp
param set SENS_FLOW_ROT 0
param set SENS_FLOW_MINHGT 0.01
param set SENS_FLOW_MAXHGT 4.0
param set SENS_FLOW_MAXR 10.0
param set-default SENS_FLOW_ROT 0
param set-default SENS_FLOW_MINHGT 0.0
param set-default SENS_FLOW_MAXHGT 4.0
param set-default SENS_FLOW_MAXR 10.0
param set EKF2_AID_MASK 27 # gps + flow + vis pos + vis yaw
param set EKF2_OF_DELAY 0
param set EKF2_OF_QMIN 10
param set EKF2_OF_N_MIN 0.05
param set EKF2_OF_N_MAX 0.2
param set EKF2_HGT_MODE 2
param set EKF2_EVA_NOISE 0.1
param set EKF2_EVP_NOISE 0.1
param set EKF2_EV_DELAY 0
param set-default EKF2_AID_MASK 26 # flow + vis pos + vis yaw
param set-default EKF2_OF_DELAY 0
param set-default EKF2_OF_QMIN 10
param set-default EKF2_OF_N_MIN 0.05
param set-default EKF2_OF_N_MAX 0.2
param set-default EKF2_HGT_MODE 2 # 0 = baro, 1 = gps, 2 = range, 3 = vision
param set-default EKF2_EVA_NOISE 0.1
param set-default EKF2_EVP_NOISE 0.1
param set-default EKF2_EV_DELAY 0

4
clover_simulation/launch/simulator.launch
View File

@@ -21,7 +21,7 @@
</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)" unless="$(eval type == 'none')">
<node name="sitl_$(arg mav_id)" pkg="px4" type="px4" output="screen" required="true" args="$(find px4)/ROMFS/px4fmu_common -s etc/init.d-posix/rcS -i $(arg mav_id)" unless="$(eval type == 'none')">
<env name="PX4_SIM_MODEL" value="$(arg vehicle)"/>
<env name="PX4_ESTIMATOR" value="$(arg est)"/>
</node>
@@ -36,7 +36,7 @@
<arg name="use_clover_physics" value="$(arg use_clover_physics)"/>
</include>
<node name="jmavsim" pkg="px4" type="jmavsim_run.sh" output="screen" if="$(eval type == 'jmavsim')">
<node name="jmavsim" pkg="px4" required="true" type="jmavsim_run.sh" output="screen" if="$(eval type == 'jmavsim')">
<env name="HEADLESS" value="1" if="$(eval not gui)"/>
</node>

2
clover_simulation/package.xml
View File

@@ -1,6 +1,6 @@
<package format="2">
<name>clover_simulation</name>
<version>0.21.1</version>
<version>0.23.0</version>
<description>The clover_simulation package provides worlds and launch files for Gazebo.</description>
<maintainer email="okalachev@gmail.com">Oleg Kalachev</maintainer>

44
clover_simulation/src/sim_leds.cpp
View File

@@ -49,14 +49,9 @@ private:
std::unique_ptr<ros::NodeHandle> nh;
ros::ServiceServer setLedsSrv;
// Note: LED state should only be published by the /gazebo node
led_msgs::LEDStateArray ledState;
ros::Publisher statePublisher;
// LED state will be read from the topic to avoid creating more services
ros::Subscriber stateSubscriber;
bool setLeds(led_msgs::SetLEDs::Request& req, led_msgs::SetLEDs::Response& resp);
void handleLedsMsg(const led_msgs::LEDStateArrayConstPtr& leds);
public:
@@ -73,16 +68,8 @@ public:
ROS_INFO_NAMED(("LedController_" + robotNamespace).c_str(), "LedController has started (as %s)", role == Role::Client ? "client" : "server");
nh.reset(new ros::NodeHandle(robotNamespace));
if (role == Role::Server)
{
setLedsSrv = nh->advertiseService("led/set_leds", &LedController::setLeds, this);
statePublisher = nh->advertise<led_msgs::LEDStateArray>("led/state", 1, true);
}
else
{
// LED state should be published to the "led/state" topic, so we grab our data there
stateSubscriber = nh->subscribe<led_msgs::LEDStateArray>("led/state", 1, &LedController::handleLedsMsg, this);
}
stateSubscriber = nh->subscribe<led_msgs::LEDStateArray>("led/state", 1, &LedController::handleLedsMsg, this);
};
~LedController()
@@ -96,13 +83,9 @@ public:
std::lock_guard<std::mutex> lock(registryMutex);
if (totalLeds > 0) {
registeredLeds.resize(totalLeds);
ledState.leds.resize(totalLeds);
}
ROS_DEBUG_NAMED(("LedController_" + robotNamespace).c_str(), "Registering LED visual plugin to %s (LED id=%d)", (role == Role::Client) ? "client" : "server", ledIdx);
registeredLeds[ledIdx] = plugin;
ledState.leds[ledIdx].index = ledIdx;
if (role == Role::Server)
statePublisher.publish(ledState);
}
void unregisterPlugin(sim_led::LedVisualPlugin* plugin)
@@ -157,7 +140,8 @@ public:
{
auto indexStr = parentName.substr(lastDashPos + 1);
try {
myIndex = std::stoi(indexStr);
if (indexStr == "visual") myIndex = 0; // the first visual doesn't have index
else myIndex = std::stoi(indexStr);
} catch(const std::exception &e) {
gzwarn << "Failed to convert " << indexStr << " to integer: " << e.what() << ", assuming 0\n";
myIndex = 0;
@@ -195,26 +179,6 @@ public:
};
}
// FIXME: These two functions basically do the same thing, maybe they can be merged?
bool led_controller::LedController::setLeds(led_msgs::SetLEDs::Request &req, led_msgs::SetLEDs::Response &resp)
{
std::lock_guard<std::mutex> lock(registryMutex);
for(const auto& led : req.leds)
{
if (led.index < registeredLeds.size()) {
auto color = GazeboColor(led.r / 255.0f, led.g / 255.0f, led.b / 255.0f);
auto ledPlugin = registeredLeds[led.index];
if (ledPlugin) ledPlugin->SetColor(color);
ledState.leds[led.index].r = led.r;
ledState.leds[led.index].g = led.g;
ledState.leds[led.index].b = led.b;
}
}
statePublisher.publish(ledState);
resp.success = true;
return true;
}
void led_controller::LedController::handleLedsMsg(const led_msgs::LEDStateArrayConstPtr& leds)
{
std::lock_guard<std::mutex> lock(registryMutex);

71
clover_simulation/src/sim_leds_controller.cpp Normal file
View File

@@ -0,0 +1,71 @@
#include <led_msgs/SetLEDs.h>
#include <led_msgs/LEDStateArray.h>
#include <ros/ros.h>
#include <gazebo/gazebo.hh>
#include <gazebo/physics/physics.hh>
#include <gazebo/common/common.hh>
class LedControllerPlugin : public gazebo::ModelPlugin {
private:
std::unique_ptr<ros::NodeHandle> nh;
std::string ns;
ros::ServiceServer setLedsSrv;
led_msgs::LEDStateArray ledState;
ros::Publisher statePublisher;
std::mutex handleMutex;
public:
bool setLeds(led_msgs::SetLEDs::Request &req, led_msgs::SetLEDs::Response &resp)
{
std::lock_guard<std::mutex> lock(handleMutex);
for(const auto& led : req.leds)
{
if (led.index < ledState.leds.size()) {
ledState.leds[led.index].r = led.r;
ledState.leds[led.index].g = led.g;
ledState.leds[led.index].b = led.b;
}
}
statePublisher.publish(ledState);
resp.success = true;
return true;
}
virtual void Load(gazebo::physics::ModelPtr model, sdf::ElementPtr sdf) override
{
ROS_INFO("Initialize LED Controller");
// We need "libgazebo_ros_api.so" to be loaded
if (!ros::isInitialized())
{
ROS_FATAL_NAMED("LedController", "Tried to load ROS plugin when ROS Gazebo API is not loaded. Please use gazebo_ros node to"
"launch Gazebo.");
}
ns = "";
if (sdf->HasElement("robotNamespace")) {
ns = sdf->Get<std::string>("robotNamespace");
}
if (!sdf->HasElement("ledCount")) {
gzerr << "ledCount is not set, but is required for the plugin to function correctly\n";
return;
}
int totalLeds = sdf->Get<int>("ledCount");
ledState.leds.resize(totalLeds);
for (int i = 0; i < totalLeds; i++) {
ledState.leds[i].index = i;
}
nh.reset(new ros::NodeHandle(ns));
setLedsSrv = nh->advertiseService("led/set_leds", &LedControllerPlugin::setLeds, this);
statePublisher = nh->advertise<led_msgs::LEDStateArray>("led/state", 1, true);
statePublisher.publish(ledState);
}
};
GZ_REGISTER_MODEL_PLUGIN(LedControllerPlugin);

BIN
docs/assets/butterfly.png
View File

Binary file not shown.
Side by Side Swipe Overlay

Before

Width:  |  Height:  |  Size: 695 KiB

After

Width:  |  Height:  |  Size: 220 KiB

BIN
docs/assets/c305.jpg Normal file
View File

Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 40 KiB

BIN
docs/assets/c4s/antennac4s.jpg Normal file
View File

Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 25 KiB

BIN
docs/assets/c4s/arecibo_broken_c4s.jpg Normal file
View File

Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 299 KiB

BIN
docs/assets/c4s/arecibo_c4s.jpg Normal file
View File

Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 152 KiB

BIN
docs/assets/c4s/code_c4s.png Normal file
View File

Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 148 KiB

BIN
docs/assets/c4s/drone_c4s.jpg Normal file
View File

Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 37 KiB

BIN
docs/assets/c4s/logo_c4s.jpg Normal file
View File

Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 6.4 KiB

BIN
docs/assets/c4s/module2_c4s.jpg Normal file
View File

Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 23 KiB

BIN
docs/assets/c4s/module_c4s.jpg Normal file
View File

Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 28 KiB

BIN
docs/assets/c4s/pic1_c4s.jpg Normal file
View File

Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 120 KiB

BIN
docs/assets/c4s/pic2_c4s.jpg Normal file
View File

Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 121 KiB

BIN
docs/assets/c4s/pic3_c4s.jpg Normal file
View File

Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 267 KiB

BIN
docs/assets/c4s/pic4_c4s.jpg Normal file
View File

Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 175 KiB

BIN
docs/assets/c4s/shot_c4s.jpg Normal file
View File

Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 265 KiB

BIN
docs/assets/c4s/string_c4s.jpg Normal file
View File

Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 4.6 KiB

BIN
docs/assets/c4s/white_noise_c4s.jpg Normal file
View File

Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 424 KiB

BIN
docs/assets/clover-rescue-team/allsettings.png Normal file
View File

Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 33 KiB

BIN
docs/assets/clover-rescue-team/bot1.jpg Normal file
View File

Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 54 KiB

BIN
docs/assets/clover-rescue-team/bot2.png Normal file
View File

Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 99 KiB

BIN
docs/assets/clover-rescue-team/bot3.jpg Normal file
View File

Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 48 KiB

BIN
docs/assets/clover-rescue-team/bot4.png Normal file
View File

Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 70 KiB

BIN
docs/assets/clover-rescue-team/bot5.jpg Normal file
View File

Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 67 KiB

BIN
docs/assets/clover-rescue-team/bot6.jpg Normal file
View File

Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 80 KiB

BIN
docs/assets/clover-rescue-team/bot7.jpg Normal file
View File

Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 42 KiB

BIN
docs/assets/clover-rescue-team/m1.jpg Normal file
View File

Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 36 KiB

BIN
docs/assets/clover-rescue-team/m2.jpg Normal file
View File

Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 35 KiB

BIN
docs/assets/clover-rescue-team/m3.jpg Normal file
View File

Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 97 KiB

BIN
docs/assets/clover-rescue-team/m4.jpg Normal file
View File

Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 34 KiB

BIN
docs/assets/clover-rescue-team/m5.jpg Normal file
View File

Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 41 KiB

BIN
docs/assets/clover-rescue-team/main.png Normal file
View File

Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 169 KiB

BIN
docs/assets/clover-rescue-team/mockup.png Normal file
View File

Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 99 KiB

BIN
docs/assets/clover-rescue-team/s1.png Normal file
View File

Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 1.7 KiB

BIN
docs/assets/clover-rescue-team/s2.png Normal file
View File

Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 4.7 KiB

BIN
docs/assets/clover-rescue-team/s3.png Normal file
View File

Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 8.1 KiB

BIN
docs/assets/clover-rescue-team/s4.png Normal file
View File

Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 27 KiB

Some files were not shown because too many files have changed in this diff Show More