mirror of
https://github.com/CopterExpress/clover.git
synced 2026-06-01 07:29:32 +00:00
Compare commits
1 Commits
rtl8812au
...
web-params
| Author | SHA1 | Date | |
|---|---|---|---|
|
2930eb9c22 |
1
.gitattributes
vendored
1
.gitattributes
vendored
@@ -3,7 +3,6 @@ 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
50
.github/workflows/build.yml
vendored
@@ -7,45 +7,17 @@ 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
|
||||
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
|
||||
- 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
|
||||
|
||||
55
.github/workflows/docs.yml
vendored
55
.github/workflows/docs.yml
vendored
@@ -4,20 +4,11 @@ on:
|
||||
push:
|
||||
branches: [ '*' ]
|
||||
pull_request:
|
||||
branches: [ '*' ]
|
||||
|
||||
permissions:
|
||||
contents: read
|
||||
pages: write
|
||||
id-token: write
|
||||
|
||||
defaults:
|
||||
run:
|
||||
shell: bash
|
||||
branches: [ master ]
|
||||
|
||||
jobs:
|
||||
docs:
|
||||
runs-on: ubuntu-22.04
|
||||
runs-on: ubuntu-18.04
|
||||
steps:
|
||||
- uses: actions/checkout@v2
|
||||
- name: Use Node.js
|
||||
@@ -43,11 +34,7 @@ jobs:
|
||||
gitbook install
|
||||
gitbook build
|
||||
- name: Generate PDF
|
||||
id: generate-pdf
|
||||
env:
|
||||
GITBOOK_SKIP_PDF: ${{ secrets.GITBOOK_SKIP_PDF }}
|
||||
continue-on-error: ${{ env.GITBOOK_SKIP_PDF != '' }}
|
||||
if: ${{ github.event_name == 'push' }}
|
||||
if: ${{ github.event_name == 'push' && github.ref == 'refs/heads/master' }}
|
||||
run: |
|
||||
for i in 1 2 3 4; do gitbook pdf ./ _book/clover.pdf && break || sleep 1; done
|
||||
sudo apt-get -q install ghostscript
|
||||
@@ -56,33 +43,11 @@ 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
|
||||
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
|
||||
- name: Deploy
|
||||
uses: JamesIves/github-pages-deploy-action@4.1.3
|
||||
if: ${{ github.event_name == 'push' && github.ref == 'refs/heads/master' }}
|
||||
with:
|
||||
path: _book
|
||||
|
||||
deploy-docs:
|
||||
if: ${{ github.event_name == 'push' && github.ref == 'refs/heads/master' }}
|
||||
concurrency:
|
||||
group: "pages"
|
||||
cancel-in-progress: true
|
||||
environment:
|
||||
name: github-pages
|
||||
url: ${{ steps.deployment.outputs.page_url }}
|
||||
runs-on: ubuntu-latest
|
||||
needs: docs
|
||||
steps:
|
||||
- name: Deploy to GitHub Pages
|
||||
env:
|
||||
FREEZE_DOCS: ${{ secrets.FREEZE_DOCS }}
|
||||
if: ${{ !env.FREEZE_DOCS }}
|
||||
id: deployment
|
||||
uses: actions/deploy-pages@v1
|
||||
branch: gh-pages
|
||||
folder: _book
|
||||
clean: true
|
||||
single-commit: true # to avoid multiple copies of large pdf files
|
||||
|
||||
2
.github/workflows/editorconfig.yaml
vendored
2
.github/workflows/editorconfig.yaml
vendored
@@ -15,4 +15,4 @@ jobs:
|
||||
run: |
|
||||
wget --no-verbose https://github.com/okalachev/editorconfig-checker/releases/download/1.2.1-disable-spaces-amount/ec-linux-amd64
|
||||
chmod +x ec-linux-amd64
|
||||
./ec-linux-amd64 -spaces-after-tabs -e "roslib.js|ros3d.js|eventemitter2.js|json-to-pretty-yaml.js|draw.cpp|BinUtils.swift|\.idea|apps/android/app|blockly/|clover_blocks/programs/|highlight/|python.js|Assets.xcassets|test_parser_pass.txt|test_node_failure.txt|aruco_pose/vendor|\.stl|\.dxf|\.dae|\.material"
|
||||
./ec-linux-amd64 -spaces-after-tabs -e "roslib.js|ros3d.js|eventemitter2.js|json-to-pretty-yaml.js|draw.cpp|BinUtils.swift|\.idea|apps/android/app|blockly/|clover_blocks/programs/|highlight/|python.js|Assets.xcassets|test_parser_pass.txt|test_node_failure.txt|aruco_pose/vendor|\.stl|\.dxf|\.dae"
|
||||
|
||||
@@ -20,7 +20,7 @@ Clover drone is used on a wide range of educational events, including [Copter Ha
|
||||
|
||||
Preconfigured image for Raspberry Pi with installed and configured software, ready to fly, is available [in the Releases section](https://github.com/CopterExpress/clover/releases).
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
Image features:
|
||||
|
||||
@@ -83,10 +83,11 @@ add_message_files(
|
||||
)
|
||||
|
||||
## Generate services in the 'srv' folder
|
||||
add_service_files(
|
||||
FILES
|
||||
SetMarkers.srv
|
||||
)
|
||||
# add_service_files(
|
||||
# FILES
|
||||
# Service1.srv
|
||||
# Service2.srv
|
||||
# )
|
||||
|
||||
## Generate actions in the 'action' folder
|
||||
# add_action_files(
|
||||
@@ -119,7 +120,6 @@ generate_messages(
|
||||
## Generate dynamic reconfigure parameters in the 'cfg' folder
|
||||
generate_dynamic_reconfigure_options(
|
||||
cfg/Detector.cfg
|
||||
cfg/Map.cfg
|
||||
)
|
||||
|
||||
###################################
|
||||
@@ -251,5 +251,4 @@ 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()
|
||||
|
||||
@@ -43,8 +43,7 @@ It's recommended to run it within the same nodelet manager with the camera nodel
|
||||
* `~frame_id_prefix` (*string*) – prefix for TF transforms names, marker's ID is appended (default: `aruco_`)
|
||||
* `~length` (*double*) – markers' sides length
|
||||
* `~length_override` (*map*) – lengths of markers with specified ids
|
||||
* `~known_vertical` (*string*) – known vertical (Z axis) of all the markers as a frame
|
||||
* `~flip_vertical` – flip vertical vector
|
||||
* `~known_tilt` (*string*) – known tilt (pitch and roll) of all the markers as a frame
|
||||
|
||||
### Topics
|
||||
|
||||
@@ -52,7 +51,6 @@ 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
|
||||
|
||||
@@ -72,12 +70,10 @@ It's recommended to run it within the same nodelet manager with the camera nodel
|
||||
|
||||
* `~map` – path to text file with markers list
|
||||
* `~frame_id` – published frame id (default: `aruco_map`)
|
||||
* `~known_vertical` – known vertical (Z axis) of markers map as a frame
|
||||
* `~flip_vertical` – flip vertical vector
|
||||
* `~known_tilt` – known tilt (pitch and roll) of markers map as a frame
|
||||
* `~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:
|
||||
@@ -101,7 +97,6 @@ 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
|
||||
|
||||
@@ -10,8 +10,6 @@ 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)
|
||||
|
||||
@@ -1,14 +0,0 @@
|
||||
#!/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,4 +1,3 @@
|
||||
# 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,4 +1,3 @@
|
||||
# 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,4 +1,3 @@
|
||||
# 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
|
||||
|
||||
@@ -1,7 +1,7 @@
|
||||
<?xml version="1.0"?>
|
||||
<package format="3">
|
||||
<package format="2">
|
||||
<name>aruco_pose</name>
|
||||
<version>0.24.0</version>
|
||||
<version>0.21.1</version>
|
||||
<description>Positioning with ArUco markers</description>
|
||||
|
||||
<maintainer email="okalachev@gmail.com">Oleg Kalachev</maintainer>
|
||||
@@ -28,8 +28,6 @@
|
||||
<depend>sensor_msgs</depend>
|
||||
<depend>rostest</depend>
|
||||
<depend>dynamic_reconfigure</depend>
|
||||
<depend condition="$ROS_PYTHON_VERSION == 2">python-docopt</depend>
|
||||
<depend condition="$ROS_PYTHON_VERSION == 3">python3-docopt</depend>
|
||||
|
||||
<test_depend>image_publisher</test_depend>
|
||||
<test_depend>ros_pytest</test_depend>
|
||||
|
||||
@@ -48,9 +48,7 @@
|
||||
#include <aruco_pose/Marker.h>
|
||||
#include <aruco_pose/MarkerArray.h>
|
||||
#include <aruco_pose/DetectorConfig.h>
|
||||
#include <aruco_pose/SetMarkers.h>
|
||||
|
||||
#include "draw.h"
|
||||
#include "utils.h"
|
||||
#include <memory>
|
||||
#include <functional>
|
||||
@@ -71,13 +69,10 @@ private:
|
||||
image_transport::CameraSubscriber img_sub_;
|
||||
ros::Publisher markers_pub_, vis_markers_pub_;
|
||||
ros::Subscriber map_markers_sub_;
|
||||
ros::ServiceServer set_markers_srv_;
|
||||
bool estimate_poses_, send_tf_, flip_vertical_, auto_flip_, use_map_markers_;
|
||||
bool waiting_for_map_;
|
||||
bool estimate_poses_, send_tf_, auto_flip_;
|
||||
double length_;
|
||||
ros::Duration transform_timeout_;
|
||||
std::unordered_map<int, double> length_override_;
|
||||
std::string frame_id_prefix_, known_vertical_;
|
||||
std::string frame_id_prefix_, known_tilt_;
|
||||
Mat camera_matrix_, dist_coeffs_;
|
||||
aruco_pose::MarkerArray array_;
|
||||
std::unordered_set<int> map_markers_ids_;
|
||||
@@ -97,17 +92,13 @@ 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_vertical_ = nh_priv_.param("known_vertical", nh_priv_.param("known_tilt", std::string(""))); // known_tilt is an old name
|
||||
flip_vertical_ = nh_priv_.param<bool>("flip_vertical", false);
|
||||
known_tilt_ = nh_priv_.param<std::string>("known_tilt", "");
|
||||
auto_flip_ = nh_priv_.param("auto_flip", false);
|
||||
|
||||
frame_id_prefix_ = nh_priv_.param<std::string>("frame_id_prefix", "aruco_");
|
||||
@@ -123,8 +114,6 @@ 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);
|
||||
@@ -138,15 +127,14 @@ 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)->image;
|
||||
Mat image = cv_bridge::toCvShare(msg, "bgr8")->image;
|
||||
|
||||
vector<int> ids;
|
||||
vector<vector<cv::Point2f>> corners, rejected;
|
||||
vector<cv::Vec3d> rvecs, tvecs;
|
||||
vector<cv::Point3f> obj_points;
|
||||
geometry_msgs::TransformStamped vertical;
|
||||
geometry_msgs::TransformStamped snap_to;
|
||||
|
||||
// Detect markers
|
||||
cv::aruco::detectMarkers(image, dictionary_, corners, ids, parameters_, rejected);
|
||||
@@ -181,20 +169,18 @@ private:
|
||||
}
|
||||
}
|
||||
|
||||
if (!known_vertical_.empty()) {
|
||||
if (!known_tilt_.empty()) {
|
||||
try {
|
||||
vertical = tf_buffer_->lookupTransform(msg->header.frame_id, known_vertical_,
|
||||
msg->header.stamp, transform_timeout_);
|
||||
snap_to = tf_buffer_->lookupTransform(msg->header.frame_id, known_tilt_,
|
||||
msg->header.stamp, ros::Duration(0.02));
|
||||
} catch (const tf2::TransformException& e) {
|
||||
NODELET_WARN_THROTTLE(5, "can't retrieve known vertical: %s", e.what());
|
||||
NODELET_WARN_THROTTLE(5, "can't snap: %s", e.what());
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
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;
|
||||
@@ -207,38 +193,25 @@ private:
|
||||
if (estimate_poses_) {
|
||||
fillPose(marker.pose, rvecs[i], tvecs[i]);
|
||||
|
||||
// apply known vertical (if enabled and vertical frame available)
|
||||
if (!known_vertical_.empty() && !vertical.header.frame_id.empty()) {
|
||||
applyVertical(marker.pose.orientation, vertical.transform.rotation, false, auto_flip_);
|
||||
// snap orientation (if enabled and snap frame available)
|
||||
if (!known_tilt_.empty() && !snap_to.header.frame_id.empty()) {
|
||||
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()) {
|
||||
// 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);
|
||||
}
|
||||
transform.transform.rotation = marker.pose.orientation;
|
||||
fillTranslation(transform.transform.translation, tvecs[i]);
|
||||
br_->sendTransform(transform);
|
||||
}
|
||||
}
|
||||
}
|
||||
array_.markers.push_back(marker);
|
||||
}
|
||||
|
||||
if (send_tf_) {
|
||||
br_->sendTransform(transforms);
|
||||
}
|
||||
}
|
||||
|
||||
markers_pub_.publish(array_);
|
||||
@@ -265,7 +238,8 @@ private:
|
||||
cv::aruco::drawDetectedMarkers(debug, corners, ids); // draw markers
|
||||
if (estimate_poses_)
|
||||
for (unsigned int i = 0; i < ids.size(); i++)
|
||||
_drawAxis(debug, camera_matrix_, dist_coeffs_, rvecs[i], tvecs[i], getMarkerLength(ids[i]));
|
||||
cv::aruco::drawAxis(debug, camera_matrix_, dist_coeffs_,
|
||||
rvecs[i], tvecs[i], getMarkerLength(ids[i]));
|
||||
|
||||
cv_bridge::CvImage out_msg;
|
||||
out_msg.header.frame_id = msg->header.frame_id;
|
||||
@@ -372,47 +346,17 @@ 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 && config.length > 0;
|
||||
length_ = config.length;
|
||||
enabled_ = config.enabled;
|
||||
parameters_->adaptiveThreshConstant = config.adaptiveThreshConstant;
|
||||
parameters_->adaptiveThreshWinSizeMin = config.adaptiveThreshWinSizeMin;
|
||||
parameters_->adaptiveThreshWinSizeMax = config.adaptiveThreshWinSizeMax;
|
||||
|
||||
@@ -19,13 +19,11 @@
|
||||
#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>
|
||||
@@ -43,7 +41,6 @@
|
||||
|
||||
#include <aruco_pose/MarkerArray.h>
|
||||
#include <aruco_pose/Marker.h>
|
||||
#include <aruco_pose/MapConfig.h>
|
||||
|
||||
#include <opencv2/opencv.hpp>
|
||||
#include <opencv2/aruco.hpp>
|
||||
@@ -77,13 +74,10 @@ 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_vertical_, map_, markers_frame_, markers_parent_frame_;
|
||||
std::string known_tilt_, map_, markers_frame_, markers_parent_frame_;
|
||||
int image_width_, image_height_, image_margin_;
|
||||
bool flip_vertical_, auto_flip_, image_axis_, put_markers_count_to_covariance_;
|
||||
bool auto_flip_, image_axis_;
|
||||
|
||||
public:
|
||||
virtual void onInit()
|
||||
@@ -95,35 +89,34 @@ 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>("map", 1, true);
|
||||
markers_pub_ = nh_priv_.advertise<aruco_pose::MarkerArray>("markers", 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);
|
||||
|
||||
type_ = nh_priv_.param<std::string>("type", "map");
|
||||
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_vertical_ = nh_priv_.param("known_vertical", nh_priv_.param("known_tilt", std::string(""))); // known_tilt is an old name
|
||||
flip_vertical_ = nh_priv_.param<bool>("flip_vertical", false);
|
||||
known_tilt_ = nh_priv_.param<std::string>("known_tilt", "");
|
||||
auto_flip_ = nh_priv_.param("auto_flip", false);
|
||||
image_width_ = nh_priv_.param("image_width" , 2000);
|
||||
image_height_ = nh_priv_.param("image_height", 2000);
|
||||
image_margin_ = nh_priv_.param("image_margin", 200);
|
||||
image_axis_ = nh_priv_.param("image_axis", true);
|
||||
put_markers_count_to_covariance_ = nh_priv_.param("put_markers_count_to_covariance", false);
|
||||
markers_parent_frame_ = nh_priv_.param<std::string>("markers/frame_id", transform_.child_frame_id);
|
||||
markers_frame_ = nh_priv_.param<std::string>("markers/child_frame_id_prefix", "");
|
||||
|
||||
// createStripLine();
|
||||
|
||||
if (type_ == "map") {
|
||||
map_ = nh_priv_.param<std::string>("map" , "");
|
||||
loadMap(map_);
|
||||
} else if (type_ == "gridboard") {
|
||||
if (type == "map") {
|
||||
param(nh_priv_, "map", 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();
|
||||
}
|
||||
|
||||
@@ -131,7 +124,10 @@ public:
|
||||
vis_markers_pub_ = nh_priv_.advertise<visualization_msgs::MarkerArray>("visualization", 1, true);
|
||||
debug_pub_ = it_priv.advertise("debug", 1);
|
||||
|
||||
publishMap();
|
||||
publishMarkersFrames();
|
||||
publishMarkers();
|
||||
publishMapImage();
|
||||
vis_markers_pub_.publish(vis_array_);
|
||||
|
||||
image_sub_.subscribe(nh_, "image_raw", 1);
|
||||
info_sub_.subscribe(nh_, "camera_info", 1);
|
||||
@@ -140,12 +136,6 @@ 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");
|
||||
}
|
||||
|
||||
@@ -153,9 +143,6 @@ 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;
|
||||
@@ -179,21 +166,7 @@ public:
|
||||
corners.push_back(marker_corners);
|
||||
}
|
||||
|
||||
if (put_markers_count_to_covariance_) {
|
||||
// HACK: pass markers count using covariance field
|
||||
int valid_markers = 0;
|
||||
for (auto const &marker : markers->markers) {
|
||||
for (auto const &board_marker : board_->ids) {
|
||||
if (board_marker == marker.id) {
|
||||
valid_markers++;
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
pose_.pose.covariance[0] = valid_markers;
|
||||
}
|
||||
|
||||
if (known_vertical_.empty()) {
|
||||
if (known_tilt_.empty()) {
|
||||
// simple estimation
|
||||
valid = cv::aruco::estimatePoseBoard(corners, ids, board_, camera_matrix_, dist_coeffs_,
|
||||
rvec, tvec, false);
|
||||
@@ -207,7 +180,7 @@ public:
|
||||
|
||||
} else {
|
||||
Mat obj_points, img_points;
|
||||
// estimation with known vertical
|
||||
// estimation with "snapping"
|
||||
cv::aruco::getBoardObjectAndImagePoints(board_, corners, ids, obj_points, img_points);
|
||||
if (obj_points.empty()) goto publish_debug;
|
||||
|
||||
@@ -219,11 +192,11 @@ public:
|
||||
|
||||
fillTransform(transform_.transform, rvec, tvec);
|
||||
try {
|
||||
geometry_msgs::TransformStamped vertical = tf_buffer_.lookupTransform(markers->header.frame_id,
|
||||
known_vertical_, markers->header.stamp, ros::Duration(0.02));
|
||||
applyVertical(transform_.transform.rotation, vertical.transform.rotation, flip_vertical_, auto_flip_);
|
||||
geometry_msgs::TransformStamped snap_to = tf_buffer_.lookupTransform(markers->header.frame_id,
|
||||
known_tilt_, markers->header.stamp, ros::Duration(0.02));
|
||||
snapOrientation(transform_.transform.rotation, snap_to.transform.rotation, auto_flip_);
|
||||
} catch (const tf2::TransformException& e) {
|
||||
NODELET_WARN_THROTTLE(1, "can't retrieve known vertical: %s", e.what());
|
||||
NODELET_WARN_THROTTLE(1, "can't snap: %s", e.what());
|
||||
}
|
||||
|
||||
geometry_msgs::TransformStamped shift;
|
||||
@@ -295,17 +268,9 @@ publish_debug:
|
||||
std::ifstream f(filename);
|
||||
std::string line;
|
||||
|
||||
clearMarkers();
|
||||
|
||||
if (map_ == "") {
|
||||
NODELET_INFO("No map loaded");
|
||||
return;
|
||||
}
|
||||
|
||||
if (!f.good()) {
|
||||
NODELET_ERROR("%s - %s", strerror(errno), filename.c_str());
|
||||
map_ = "";
|
||||
return;
|
||||
NODELET_FATAL("%s - %s", strerror(errno), filename.c_str());
|
||||
ros::shutdown();
|
||||
}
|
||||
|
||||
while (std::getline(f, line)) {
|
||||
@@ -331,10 +296,9 @@ publish_debug:
|
||||
s.putback(first);
|
||||
} else {
|
||||
// Probably garbage data; inform user and throw an exception, possibly killing nodelet
|
||||
NODELET_ERROR("Malformed input: %s", line.c_str());
|
||||
map_ = "";
|
||||
clearMarkers();
|
||||
return;
|
||||
NODELET_FATAL("Malformed input: %s", line.c_str());
|
||||
ros::shutdown();
|
||||
throw std::runtime_error("Malformed input");
|
||||
}
|
||||
|
||||
if (!(s >> id >> length >> x >> y)) {
|
||||
@@ -365,14 +329,6 @@ 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");
|
||||
@@ -414,15 +370,6 @@ 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;
|
||||
@@ -519,7 +466,7 @@ publish_debug:
|
||||
vis_marker.pose.position.x = x;
|
||||
vis_marker.pose.position.y = y;
|
||||
vis_marker.pose.position.z = z;
|
||||
tf::quaternionTFToMsg(q, vis_marker.pose.orientation);
|
||||
tf::quaternionTFToMsg(q, marker.pose.orientation);
|
||||
vis_marker.frame_locked = true;
|
||||
vis_array_.markers.push_back(vis_marker);
|
||||
|
||||
@@ -562,22 +509,6 @@ 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)
|
||||
|
||||
@@ -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 -o test_map.txt
|
||||
rosrun aruco_pose genmap.py 0.33 2 4 1 1 0 > $(catkin_find aruco_pose map)/test_map.txt
|
||||
"""
|
||||
|
||||
from __future__ import print_function
|
||||
|
||||
@@ -106,25 +106,26 @@ inline bool isFlipped(tf::Quaternion& q)
|
||||
return (abs(pitch) > M_PI / 2) || (abs(roll) > M_PI / 2);
|
||||
}
|
||||
|
||||
/* Apply a vertical to an orientation */
|
||||
inline void applyVertical(geometry_msgs::Quaternion& orientation, const geometry_msgs::Quaternion& vertical,
|
||||
bool flip_vertical = false, bool auto_flip = false) // editorconfig-checker-disable-line
|
||||
/* Set roll and pitch from "from" to "to", keeping yaw */
|
||||
inline void snapOrientation(geometry_msgs::Quaternion& to, const geometry_msgs::Quaternion& from, bool auto_flip = false)
|
||||
{
|
||||
tf::Quaternion _vertical, _orientation;
|
||||
tf::quaternionMsgToTF(vertical, _vertical);
|
||||
tf::quaternionMsgToTF(orientation, _orientation);
|
||||
tf::Quaternion _from, _to;
|
||||
tf::quaternionMsgToTF(from, _from);
|
||||
tf::quaternionMsgToTF(to, _to);
|
||||
|
||||
if (flip_vertical || (auto_flip && !isFlipped(_orientation))) {
|
||||
static const tf::Quaternion flip = tf::createQuaternionFromRPY(M_PI, 0, 0);
|
||||
_vertical *= flip; // flip vertical
|
||||
if (auto_flip) {
|
||||
if (!isFlipped(_from)) {
|
||||
static const tf::Quaternion flip = tf::createQuaternionFromRPY(M_PI, 0, 0);
|
||||
_from *= flip; // flip "from"
|
||||
}
|
||||
}
|
||||
|
||||
auto diff = tf::Matrix3x3(_orientation).transposeTimes(tf::Matrix3x3(_vertical));
|
||||
auto diff = tf::Matrix3x3(_to).transposeTimes(tf::Matrix3x3(_from));
|
||||
double _, yaw;
|
||||
diff.getRPY(_, _, yaw);
|
||||
auto q = tf::createQuaternionFromRPY(0, 0, -yaw);
|
||||
_vertical = _vertical * q; // set yaw from orientation to vertical
|
||||
tf::quaternionTFToMsg(_vertical, orientation); // set vertical to orientation
|
||||
_from = _from * q; // set yaw from "to" to "from"
|
||||
tf::quaternionTFToMsg(_from, to); // set "from" to "to"
|
||||
}
|
||||
|
||||
inline void transformToPose(const geometry_msgs::Transform& transform, geometry_msgs::Pose& pose)
|
||||
|
||||
@@ -1,7 +0,0 @@
|
||||
# * 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
|
||||
@@ -6,7 +6,7 @@ import tf2_geometry_msgs
|
||||
from geometry_msgs.msg import PoseWithCovarianceStamped
|
||||
from sensor_msgs.msg import Image
|
||||
from aruco_pose.msg import MarkerArray
|
||||
from visualization_msgs.msg import MarkerArray as VisMarkerArray, Marker as VisMarker
|
||||
from visualization_msgs.msg import MarkerArray as VisMarkerArray
|
||||
|
||||
|
||||
@pytest.fixture
|
||||
@@ -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/map', MarkerArray, timeout=5)
|
||||
markers = rospy.wait_for_message('aruco_map/markers', MarkerArray, timeout=5)
|
||||
assert markers.markers[0].id == 1
|
||||
assert markers.markers[1].id == 2
|
||||
assert markers.markers[2].id == 3
|
||||
@@ -199,36 +199,6 @@ def test_map_markers(node):
|
||||
|
||||
def test_map_visualization(node):
|
||||
vis = rospy.wait_for_message('aruco_map/visualization', VisMarkerArray, timeout=5)
|
||||
assert len(vis.markers) == 7
|
||||
assert vis.markers[0].header.frame_id == 'aruco_map'
|
||||
assert vis.markers[0].type == VisMarker.CUBE
|
||||
assert vis.markers[0].action == VisMarker.ADD
|
||||
assert vis.markers[0].pose.position.x == 0
|
||||
assert vis.markers[0].pose.position.y == 0
|
||||
assert vis.markers[0].pose.position.z == 0
|
||||
assert vis.markers[0].pose.orientation.x == 0
|
||||
assert vis.markers[0].pose.orientation.y == 0
|
||||
assert vis.markers[0].pose.orientation.z == 0
|
||||
assert vis.markers[0].pose.orientation.w == 1
|
||||
assert vis.markers[0].scale.x == approx(0.33)
|
||||
assert vis.markers[0].scale.y == approx(0.33)
|
||||
assert vis.markers[0].scale.z == approx(0.001)
|
||||
assert vis.markers[1].pose.position.x == 1
|
||||
assert vis.markers[1].pose.position.y == 0
|
||||
assert vis.markers[1].pose.position.z == 0
|
||||
assert vis.markers[1].pose.orientation.x == 0
|
||||
assert vis.markers[1].pose.orientation.y == 0
|
||||
assert vis.markers[1].pose.orientation.z == 0
|
||||
assert vis.markers[1].pose.orientation.w == 1
|
||||
# non-zero yaw marker:
|
||||
assert vis.markers[4].scale.x == approx(0.5)
|
||||
assert vis.markers[4].pose.position.x == approx(0.5)
|
||||
assert vis.markers[4].pose.position.y == 2
|
||||
assert vis.markers[4].pose.position.z == 0
|
||||
assert vis.markers[4].pose.orientation.x == 0
|
||||
assert vis.markers[4].pose.orientation.y == 0
|
||||
assert vis.markers[4].pose.orientation.z == approx(0.5646424733950354)
|
||||
assert vis.markers[4].pose.orientation.w == approx(0.8253356149096783)
|
||||
|
||||
def test_map_debug(node):
|
||||
img = rospy.wait_for_message('aruco_map/debug', Image, timeout=5)
|
||||
|
||||
Binary file not shown.
|
Before Width: | Height: | Size: 62 KiB |
@@ -1,8 +0,0 @@
|
||||
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'
|
||||
@@ -1,21 +0,0 @@
|
||||
<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>
|
||||
@@ -2,7 +2,6 @@ import rospy
|
||||
import pytest
|
||||
|
||||
from visualization_msgs.msg import MarkerArray as VisMarkerArray
|
||||
from aruco_pose.msg import MarkerArray
|
||||
|
||||
|
||||
@pytest.fixture
|
||||
@@ -10,5 +9,5 @@ def node():
|
||||
return rospy.init_node('aruco_pose_test', anonymous=True)
|
||||
|
||||
def test_node_failure(node):
|
||||
assert rospy.wait_for_message('aruco_map/visualization', VisMarkerArray, timeout=5).markers == []
|
||||
assert rospy.wait_for_message('aruco_map/map', MarkerArray, timeout=5).markers == []
|
||||
with pytest.raises(rospy.exceptions.ROSException):
|
||||
rospy.wait_for_message('aruco_map/visualization', VisMarkerArray, timeout=5)
|
||||
|
||||
@@ -16,723 +16,3 @@ web_video_server:
|
||||
ws281x:
|
||||
debian:
|
||||
buster: [ros-noetic-ws281x]
|
||||
catkin:
|
||||
debian:
|
||||
buster: [ros-noetic-catkin]
|
||||
genmsg:
|
||||
debian:
|
||||
buster: [ros-noetic-genmsg]
|
||||
gencpp:
|
||||
debian:
|
||||
buster: [ros-noetic-gencpp]
|
||||
geneus:
|
||||
debian:
|
||||
buster: [ros-noetic-geneus]
|
||||
genlisp:
|
||||
debian:
|
||||
buster: [ros-noetic-genlisp]
|
||||
gennodejs:
|
||||
debian:
|
||||
buster: [ros-noetic-gennodejs]
|
||||
genpy:
|
||||
debian:
|
||||
buster: [ros-noetic-genpy]
|
||||
bond_core:
|
||||
debian:
|
||||
buster: [ros-noetic-bond-core]
|
||||
cmake_modules:
|
||||
debian:
|
||||
buster: [ros-noetic-cmake-modules]
|
||||
class_loader:
|
||||
debian:
|
||||
buster: [ros-noetic-class-loader]
|
||||
common_msgs:
|
||||
debian:
|
||||
buster: [ros-noetic-common-msgs]
|
||||
common_tutorials:
|
||||
debian:
|
||||
buster: [ros-noetic-common-tutorials]
|
||||
cpp_common:
|
||||
debian:
|
||||
buster: [ros-noetic-cpp-common]
|
||||
desktop:
|
||||
debian:
|
||||
buster: [ros-noetic-desktop]
|
||||
diagnostics:
|
||||
debian:
|
||||
buster: [ros-noetic-diagnostics]
|
||||
executive_smach:
|
||||
debian:
|
||||
buster: [ros-noetic-executive-smach]
|
||||
geometry:
|
||||
debian:
|
||||
buster: [ros-noetic-geometry]
|
||||
geometry_tutorials:
|
||||
debian:
|
||||
buster: [ros-noetic-geometry-tutorials]
|
||||
gl_dependency:
|
||||
debian:
|
||||
buster: [ros-noetic-gl-dependency]
|
||||
image_common:
|
||||
debian:
|
||||
buster: [ros-noetic-image-common]
|
||||
image_pipeline:
|
||||
debian:
|
||||
buster: [ros-noetic-image-pipeline]
|
||||
image_transport_plugins:
|
||||
debian:
|
||||
buster: [ros-noetic-image-transport-plugins]
|
||||
laser_pipeline:
|
||||
debian:
|
||||
buster: [ros-noetic-laser-pipeline]
|
||||
mavlink:
|
||||
debian:
|
||||
buster: [ros-noetic-mavlink]
|
||||
media_export:
|
||||
debian:
|
||||
buster: [ros-noetic-media-export]
|
||||
message_generation:
|
||||
debian:
|
||||
buster: [ros-noetic-message-generation]
|
||||
message_runtime:
|
||||
debian:
|
||||
buster: [ros-noetic-message-runtime]
|
||||
mk:
|
||||
debian:
|
||||
buster: [ros-noetic-mk]
|
||||
nodelet_core:
|
||||
debian:
|
||||
buster: [ros-noetic-nodelet-core]
|
||||
orocos_kdl:
|
||||
debian:
|
||||
buster: [ros-noetic-orocos-kdl]
|
||||
perception:
|
||||
debian:
|
||||
buster: [ros-noetic-perception]
|
||||
perception_pcl:
|
||||
debian:
|
||||
buster: [ros-noetic-perception-pcl]
|
||||
python_orocos_kdl:
|
||||
debian:
|
||||
buster: [ros-noetic-python-orocos-kdl]
|
||||
qt_dotgraph:
|
||||
debian:
|
||||
buster: [ros-noetic-qt-dotgraph]
|
||||
qt_gui:
|
||||
debian:
|
||||
buster: [ros-noetic-qt-gui]
|
||||
qt_gui_py_common:
|
||||
debian:
|
||||
buster: [ros-noetic-qt-gui-py-common]
|
||||
qwt_dependency:
|
||||
debian:
|
||||
buster: [ros-noetic-qwt-dependency]
|
||||
robot:
|
||||
debian:
|
||||
buster: [ros-noetic-robot]
|
||||
ros:
|
||||
debian:
|
||||
buster: [ros-noetic-ros]
|
||||
ros_base:
|
||||
debian:
|
||||
buster: [ros-noetic-ros-base]
|
||||
ros_comm:
|
||||
debian:
|
||||
buster: [ros-noetic-ros-comm]
|
||||
ros_core:
|
||||
debian:
|
||||
buster: [ros-noetic-ros-core]
|
||||
ros_environment:
|
||||
debian:
|
||||
buster: [ros-noetic-ros-environment]
|
||||
ros_tutorials:
|
||||
debian:
|
||||
buster: [ros-noetic-ros-tutorials]
|
||||
rosapi:
|
||||
debian:
|
||||
buster: [ros-noetic-rosapi]
|
||||
rosbag_migration_rule:
|
||||
debian:
|
||||
buster: [ros-noetic-rosbag-migration-rule]
|
||||
rosbash:
|
||||
debian:
|
||||
buster: [ros-noetic-rosbash]
|
||||
rosboost_cfg:
|
||||
debian:
|
||||
buster: [ros-noetic-rosboost-cfg]
|
||||
rosbridge_server:
|
||||
debian:
|
||||
buster: [ros-noetic-rosbridge-server]
|
||||
rosbridge_suite:
|
||||
debian:
|
||||
buster: [ros-noetic-rosbridge-suite]
|
||||
rosbuild:
|
||||
debian:
|
||||
buster: [ros-noetic-rosbuild]
|
||||
rosclean:
|
||||
debian:
|
||||
buster: [ros-noetic-rosclean]
|
||||
roscpp_core:
|
||||
debian:
|
||||
buster: [ros-noetic-roscpp-core]
|
||||
roscpp_traits:
|
||||
debian:
|
||||
buster: [ros-noetic-roscpp-traits]
|
||||
roscreate:
|
||||
debian:
|
||||
buster: [ros-noetic-roscreate]
|
||||
rosgraph:
|
||||
debian:
|
||||
buster: [ros-noetic-rosgraph]
|
||||
roslang:
|
||||
debian:
|
||||
buster: [ros-noetic-roslang]
|
||||
roslint:
|
||||
debian:
|
||||
buster: [ros-noetic-roslint]
|
||||
roslisp:
|
||||
debian:
|
||||
buster: [ros-noetic-roslisp]
|
||||
rosmake:
|
||||
debian:
|
||||
buster: [ros-noetic-rosmake]
|
||||
rosmaster:
|
||||
debian:
|
||||
buster: [ros-noetic-rosmaster]
|
||||
rospack:
|
||||
debian:
|
||||
buster: [ros-noetic-rospack]
|
||||
roslib:
|
||||
debian:
|
||||
buster: [ros-noetic-roslib]
|
||||
rosparam:
|
||||
debian:
|
||||
buster: [ros-noetic-rosparam]
|
||||
rospy:
|
||||
debian:
|
||||
buster: [ros-noetic-rospy]
|
||||
rosserial:
|
||||
debian:
|
||||
buster: [ros-noetic-rosserial]
|
||||
rosserial_msgs:
|
||||
debian:
|
||||
buster: [ros-noetic-rosserial-msgs]
|
||||
rosserial_python:
|
||||
debian:
|
||||
buster: [ros-noetic-rosserial-python]
|
||||
rosservice:
|
||||
debian:
|
||||
buster: [ros-noetic-rosservice]
|
||||
rostime:
|
||||
debian:
|
||||
buster: [ros-noetic-rostime]
|
||||
roscpp_serialization:
|
||||
debian:
|
||||
buster: [ros-noetic-roscpp-serialization]
|
||||
python_qt_binding:
|
||||
debian:
|
||||
buster: [ros-noetic-python-qt-binding]
|
||||
roslaunch:
|
||||
debian:
|
||||
buster: [ros-noetic-roslaunch]
|
||||
rosunit:
|
||||
debian:
|
||||
buster: [ros-noetic-rosunit]
|
||||
angles:
|
||||
debian:
|
||||
buster: [ros-noetic-angles]
|
||||
libmavconn:
|
||||
debian:
|
||||
buster: [ros-noetic-libmavconn]
|
||||
rosconsole:
|
||||
debian:
|
||||
buster: [ros-noetic-rosconsole]
|
||||
pluginlib:
|
||||
debian:
|
||||
buster: [ros-noetic-pluginlib]
|
||||
qt_gui_cpp:
|
||||
debian:
|
||||
buster: [ros-noetic-qt-gui-cpp]
|
||||
resource_retriever:
|
||||
debian:
|
||||
buster: [ros-noetic-resource-retriever]
|
||||
rosconsole_bridge:
|
||||
debian:
|
||||
buster: [ros-noetic-rosconsole-bridge]
|
||||
roslz4:
|
||||
debian:
|
||||
buster: [ros-noetic-roslz4]
|
||||
rosserial_client:
|
||||
debian:
|
||||
buster: [ros-noetic-rosserial-client]
|
||||
rostest:
|
||||
debian:
|
||||
buster: [ros-noetic-rostest]
|
||||
rqt_action:
|
||||
debian:
|
||||
buster: [ros-noetic-rqt-action]
|
||||
rqt_bag:
|
||||
debian:
|
||||
buster: [ros-noetic-rqt-bag]
|
||||
rqt_bag_plugins:
|
||||
debian:
|
||||
buster: [ros-noetic-rqt-bag-plugins]
|
||||
rqt_common_plugins:
|
||||
debian:
|
||||
buster: [ros-noetic-rqt-common-plugins]
|
||||
rqt_console:
|
||||
debian:
|
||||
buster: [ros-noetic-rqt-console]
|
||||
rqt_dep:
|
||||
debian:
|
||||
buster: [ros-noetic-rqt-dep]
|
||||
rqt_graph:
|
||||
debian:
|
||||
buster: [ros-noetic-rqt-graph]
|
||||
rqt_gui:
|
||||
debian:
|
||||
buster: [ros-noetic-rqt-gui]
|
||||
rqt_logger_level:
|
||||
debian:
|
||||
buster: [ros-noetic-rqt-logger-level]
|
||||
rqt_moveit:
|
||||
debian:
|
||||
buster: [ros-noetic-rqt-moveit]
|
||||
rqt_msg:
|
||||
debian:
|
||||
buster: [ros-noetic-rqt-msg]
|
||||
rqt_nav_view:
|
||||
debian:
|
||||
buster: [ros-noetic-rqt-nav-view]
|
||||
rqt_plot:
|
||||
debian:
|
||||
buster: [ros-noetic-rqt-plot]
|
||||
rqt_pose_view:
|
||||
debian:
|
||||
buster: [ros-noetic-rqt-pose-view]
|
||||
rqt_publisher:
|
||||
debian:
|
||||
buster: [ros-noetic-rqt-publisher]
|
||||
rqt_py_console:
|
||||
debian:
|
||||
buster: [ros-noetic-rqt-py-console]
|
||||
rqt_reconfigure:
|
||||
debian:
|
||||
buster: [ros-noetic-rqt-reconfigure]
|
||||
rqt_robot_dashboard:
|
||||
debian:
|
||||
buster: [ros-noetic-rqt-robot-dashboard]
|
||||
rqt_robot_monitor:
|
||||
debian:
|
||||
buster: [ros-noetic-rqt-robot-monitor]
|
||||
rqt_robot_plugins:
|
||||
debian:
|
||||
buster: [ros-noetic-rqt-robot-plugins]
|
||||
rqt_robot_steering:
|
||||
debian:
|
||||
buster: [ros-noetic-rqt-robot-steering]
|
||||
rqt_runtime_monitor:
|
||||
debian:
|
||||
buster: [ros-noetic-rqt-runtime-monitor]
|
||||
rqt_service_caller:
|
||||
debian:
|
||||
buster: [ros-noetic-rqt-service-caller]
|
||||
rqt_shell:
|
||||
debian:
|
||||
buster: [ros-noetic-rqt-shell]
|
||||
rqt_srv:
|
||||
debian:
|
||||
buster: [ros-noetic-rqt-srv]
|
||||
rqt_tf_tree:
|
||||
debian:
|
||||
buster: [ros-noetic-rqt-tf-tree]
|
||||
rqt_top:
|
||||
debian:
|
||||
buster: [ros-noetic-rqt-top]
|
||||
rqt_topic:
|
||||
debian:
|
||||
buster: [ros-noetic-rqt-topic]
|
||||
rqt_web:
|
||||
debian:
|
||||
buster: [ros-noetic-rqt-web]
|
||||
smach:
|
||||
debian:
|
||||
buster: [ros-noetic-smach]
|
||||
smclib:
|
||||
debian:
|
||||
buster: [ros-noetic-smclib]
|
||||
std_msgs:
|
||||
debian:
|
||||
buster: [ros-noetic-std-msgs]
|
||||
actionlib_msgs:
|
||||
debian:
|
||||
buster: [ros-noetic-actionlib-msgs]
|
||||
bond:
|
||||
debian:
|
||||
buster: [ros-noetic-bond]
|
||||
diagnostic_msgs:
|
||||
debian:
|
||||
buster: [ros-noetic-diagnostic-msgs]
|
||||
geometry_msgs:
|
||||
debian:
|
||||
buster: [ros-noetic-geometry-msgs]
|
||||
eigen_conversions:
|
||||
debian:
|
||||
buster: [ros-noetic-eigen-conversions]
|
||||
kdl_conversions:
|
||||
debian:
|
||||
buster: [ros-noetic-kdl-conversions]
|
||||
nav_msgs:
|
||||
debian:
|
||||
buster: [ros-noetic-nav-msgs]
|
||||
rosbridge_msgs:
|
||||
debian:
|
||||
buster: [ros-noetic-rosbridge-msgs]
|
||||
rosgraph_msgs:
|
||||
debian:
|
||||
buster: [ros-noetic-rosgraph-msgs]
|
||||
rosmsg:
|
||||
debian:
|
||||
buster: [ros-noetic-rosmsg]
|
||||
rqt_py_common:
|
||||
debian:
|
||||
buster: [ros-noetic-rqt-py-common]
|
||||
shape_msgs:
|
||||
debian:
|
||||
buster: [ros-noetic-shape-msgs]
|
||||
smach_msgs:
|
||||
debian:
|
||||
buster: [ros-noetic-smach-msgs]
|
||||
std_srvs:
|
||||
debian:
|
||||
buster: [ros-noetic-std-srvs]
|
||||
tf2_msgs:
|
||||
debian:
|
||||
buster: [ros-noetic-tf2-msgs]
|
||||
tf2:
|
||||
debian:
|
||||
buster: [ros-noetic-tf2]
|
||||
tf2_eigen:
|
||||
debian:
|
||||
buster: [ros-noetic-tf2-eigen]
|
||||
trajectory_msgs:
|
||||
debian:
|
||||
buster: [ros-noetic-trajectory-msgs]
|
||||
control_msgs:
|
||||
debian:
|
||||
buster: [ros-noetic-control-msgs]
|
||||
urdf_parser_plugin:
|
||||
debian:
|
||||
buster: [ros-noetic-urdf-parser-plugin]
|
||||
urdfdom_py:
|
||||
debian:
|
||||
buster: [ros-noetic-urdfdom-py]
|
||||
uuid_msgs:
|
||||
debian:
|
||||
buster: [ros-noetic-uuid-msgs]
|
||||
geographic_msgs:
|
||||
debian:
|
||||
buster: [ros-noetic-geographic-msgs]
|
||||
vision_opencv:
|
||||
debian:
|
||||
buster: [ros-noetic-vision-opencv]
|
||||
visualization_msgs:
|
||||
debian:
|
||||
buster: [ros-noetic-visualization-msgs]
|
||||
visualization_tutorials:
|
||||
debian:
|
||||
buster: [ros-noetic-visualization-tutorials]
|
||||
viz:
|
||||
debian:
|
||||
buster: [ros-noetic-viz]
|
||||
webkit_dependency:
|
||||
debian:
|
||||
buster: [ros-noetic-webkit-dependency]
|
||||
xmlrpcpp:
|
||||
debian:
|
||||
buster: [ros-noetic-xmlrpcpp]
|
||||
roscpp:
|
||||
debian:
|
||||
buster: [ros-noetic-roscpp]
|
||||
bondcpp:
|
||||
debian:
|
||||
buster: [ros-noetic-bondcpp]
|
||||
bondpy:
|
||||
debian:
|
||||
buster: [ros-noetic-bondpy]
|
||||
nodelet:
|
||||
debian:
|
||||
buster: [ros-noetic-nodelet]
|
||||
nodelet_tutorial_math:
|
||||
debian:
|
||||
buster: [ros-noetic-nodelet-tutorial-math]
|
||||
pluginlib_tutorials:
|
||||
debian:
|
||||
buster: [ros-noetic-pluginlib-tutorials]
|
||||
roscpp_tutorials:
|
||||
debian:
|
||||
buster: [ros-noetic-roscpp-tutorials]
|
||||
rosout:
|
||||
debian:
|
||||
buster: [ros-noetic-rosout]
|
||||
camera_calibration:
|
||||
debian:
|
||||
buster: [ros-noetic-camera-calibration]
|
||||
diagnostic_aggregator:
|
||||
debian:
|
||||
buster: [ros-noetic-diagnostic-aggregator]
|
||||
diagnostic_updater:
|
||||
debian:
|
||||
buster: [ros-noetic-diagnostic-updater]
|
||||
diagnostic_common_diagnostics:
|
||||
debian:
|
||||
buster: [ros-noetic-diagnostic-common-diagnostics]
|
||||
dynamic_reconfigure:
|
||||
debian:
|
||||
buster: [ros-noetic-dynamic-reconfigure]
|
||||
filters:
|
||||
debian:
|
||||
buster: [ros-noetic-filters]
|
||||
joint_state_publisher:
|
||||
debian:
|
||||
buster: [ros-noetic-joint-state-publisher]
|
||||
message_filters:
|
||||
debian:
|
||||
buster: [ros-noetic-message-filters]
|
||||
rosauth:
|
||||
debian:
|
||||
buster: [ros-noetic-rosauth]
|
||||
rosbag_storage:
|
||||
debian:
|
||||
buster: [ros-noetic-rosbag-storage]
|
||||
rosnode:
|
||||
debian:
|
||||
buster: [ros-noetic-rosnode]
|
||||
rospy_tutorials:
|
||||
debian:
|
||||
buster: [ros-noetic-rospy-tutorials]
|
||||
rosshow:
|
||||
debian:
|
||||
buster: [ros-noetic-rosshow]
|
||||
rostopic:
|
||||
debian:
|
||||
buster: [ros-noetic-rostopic]
|
||||
rqt_gui_cpp:
|
||||
debian:
|
||||
buster: [ros-noetic-rqt-gui-cpp]
|
||||
rqt_gui_py:
|
||||
debian:
|
||||
buster: [ros-noetic-rqt-gui-py]
|
||||
self_test:
|
||||
debian:
|
||||
buster: [ros-noetic-self-test]
|
||||
smach_ros:
|
||||
debian:
|
||||
buster: [ros-noetic-smach-ros]
|
||||
tf2_py:
|
||||
debian:
|
||||
buster: [ros-noetic-tf2-py]
|
||||
topic_tools:
|
||||
debian:
|
||||
buster: [ros-noetic-topic-tools]
|
||||
rosbag:
|
||||
debian:
|
||||
buster: [ros-noetic-rosbag]
|
||||
actionlib:
|
||||
debian:
|
||||
buster: [ros-noetic-actionlib]
|
||||
actionlib_tutorials:
|
||||
debian:
|
||||
buster: [ros-noetic-actionlib-tutorials]
|
||||
diagnostic_analysis:
|
||||
debian:
|
||||
buster: [ros-noetic-diagnostic-analysis]
|
||||
nodelet_topic_tools:
|
||||
debian:
|
||||
buster: [ros-noetic-nodelet-topic-tools]
|
||||
roswtf:
|
||||
debian:
|
||||
buster: [ros-noetic-roswtf]
|
||||
rqt_launch:
|
||||
debian:
|
||||
buster: [ros-noetic-rqt-launch]
|
||||
sensor_msgs:
|
||||
debian:
|
||||
buster: [ros-noetic-sensor-msgs]
|
||||
camera_calibration_parsers:
|
||||
debian:
|
||||
buster: [ros-noetic-camera-calibration-parsers]
|
||||
cv_bridge:
|
||||
debian:
|
||||
buster: [ros-noetic-cv-bridge]
|
||||
image_geometry:
|
||||
debian:
|
||||
buster: [ros-noetic-image-geometry]
|
||||
image_transport:
|
||||
debian:
|
||||
buster: [ros-noetic-image-transport]
|
||||
camera_info_manager:
|
||||
debian:
|
||||
buster: [ros-noetic-camera-info-manager]
|
||||
compressed_depth_image_transport:
|
||||
debian:
|
||||
buster: [ros-noetic-compressed-depth-image-transport]
|
||||
compressed_image_transport:
|
||||
debian:
|
||||
buster: [ros-noetic-compressed-image-transport]
|
||||
cv_camera:
|
||||
debian:
|
||||
buster: [ros-noetic-cv-camera]
|
||||
image_proc:
|
||||
debian:
|
||||
buster: [ros-noetic-image-proc]
|
||||
image_publisher:
|
||||
debian:
|
||||
buster: [ros-noetic-image-publisher]
|
||||
map_msgs:
|
||||
debian:
|
||||
buster: [ros-noetic-map-msgs]
|
||||
mavros_msgs:
|
||||
debian:
|
||||
buster: [ros-noetic-mavros-msgs]
|
||||
pcl_msgs:
|
||||
debian:
|
||||
buster: [ros-noetic-pcl-msgs]
|
||||
pcl_conversions:
|
||||
debian:
|
||||
buster: [ros-noetic-pcl-conversions]
|
||||
polled_camera:
|
||||
debian:
|
||||
buster: [ros-noetic-polled-camera]
|
||||
rqt_image_view:
|
||||
debian:
|
||||
buster: [ros-noetic-rqt-image-view]
|
||||
stereo_msgs:
|
||||
debian:
|
||||
buster: [ros-noetic-stereo-msgs]
|
||||
image_view:
|
||||
debian:
|
||||
buster: [ros-noetic-image-view]
|
||||
rosbridge_library:
|
||||
debian:
|
||||
buster: [ros-noetic-rosbridge-library]
|
||||
stereo_image_proc:
|
||||
debian:
|
||||
buster: [ros-noetic-stereo-image-proc]
|
||||
tf2_ros:
|
||||
debian:
|
||||
buster: [ros-noetic-tf2-ros]
|
||||
depth_image_proc:
|
||||
debian:
|
||||
buster: [ros-noetic-depth-image-proc]
|
||||
mavros:
|
||||
debian:
|
||||
buster: [ros-noetic-mavros]
|
||||
tf:
|
||||
debian:
|
||||
buster: [ros-noetic-tf]
|
||||
interactive_markers:
|
||||
debian:
|
||||
buster: [ros-noetic-interactive-markers]
|
||||
interactive_marker_tutorials:
|
||||
debian:
|
||||
buster: [ros-noetic-interactive-marker-tutorials]
|
||||
laser_geometry:
|
||||
debian:
|
||||
buster: [ros-noetic-laser-geometry]
|
||||
laser_assembler:
|
||||
debian:
|
||||
buster: [ros-noetic-laser-assembler]
|
||||
laser_filters:
|
||||
debian:
|
||||
buster: [ros-noetic-laser-filters]
|
||||
pcl_ros:
|
||||
debian:
|
||||
buster: [ros-noetic-pcl-ros]
|
||||
tf2_geometry_msgs:
|
||||
debian:
|
||||
buster: [ros-noetic-tf2-geometry-msgs]
|
||||
image_rotate:
|
||||
debian:
|
||||
buster: [ros-noetic-image-rotate]
|
||||
tf2_kdl:
|
||||
debian:
|
||||
buster: [ros-noetic-tf2-kdl]
|
||||
tf2_web_republisher:
|
||||
debian:
|
||||
buster: [ros-noetic-tf2-web-republisher]
|
||||
tf_conversions:
|
||||
debian:
|
||||
buster: [ros-noetic-tf-conversions]
|
||||
theora_image_transport:
|
||||
debian:
|
||||
buster: [ros-noetic-theora-image-transport]
|
||||
turtlesim:
|
||||
debian:
|
||||
buster: [ros-noetic-turtlesim]
|
||||
turtle_actionlib:
|
||||
debian:
|
||||
buster: [ros-noetic-turtle-actionlib]
|
||||
turtle_tf:
|
||||
debian:
|
||||
buster: [ros-noetic-turtle-tf]
|
||||
turtle_tf2:
|
||||
debian:
|
||||
buster: [ros-noetic-turtle-tf2]
|
||||
urdf:
|
||||
debian:
|
||||
buster: [ros-noetic-urdf]
|
||||
kdl_parser:
|
||||
debian:
|
||||
buster: [ros-noetic-kdl-parser]
|
||||
kdl_parser_py:
|
||||
debian:
|
||||
buster: [ros-noetic-kdl-parser-py]
|
||||
mavros_extras:
|
||||
debian:
|
||||
buster: [ros-noetic-mavros-extras]
|
||||
robot_state_publisher:
|
||||
debian:
|
||||
buster: [ros-noetic-robot-state-publisher]
|
||||
rviz:
|
||||
debian:
|
||||
buster: [ros-noetic-rviz]
|
||||
librviz_tutorial:
|
||||
debian:
|
||||
buster: [ros-noetic-librviz-tutorial]
|
||||
rqt_rviz:
|
||||
debian:
|
||||
buster: [ros-noetic-rqt-rviz]
|
||||
rviz_plugin_tutorials:
|
||||
debian:
|
||||
buster: [ros-noetic-rviz-plugin-tutorials]
|
||||
rviz_python_tutorial:
|
||||
debian:
|
||||
buster: [ros-noetic-rviz-python-tutorial]
|
||||
urdf_tutorial:
|
||||
debian:
|
||||
buster: [ros-noetic-urdf-tutorial]
|
||||
usb_cam:
|
||||
debian:
|
||||
buster: [ros-noetic-usb-cam]
|
||||
visualization_marker_tutorials:
|
||||
debian:
|
||||
buster: [ros-noetic-visualization-marker-tutorials]
|
||||
vl53l1x:
|
||||
debian:
|
||||
buster: [ros-noetic-vl53l1x]
|
||||
xacro:
|
||||
debian:
|
||||
buster: [ros-noetic-xacro]
|
||||
ddynamic_reconfigure:
|
||||
debian:
|
||||
buster: [ros-noetic-ddynamic-reconfigure]
|
||||
librealsense2:
|
||||
debian:
|
||||
buster: [ros-noetic-librealsense2]
|
||||
realsense2_camera:
|
||||
debian:
|
||||
buster: [ros-noetic-realsense2-camera]
|
||||
realsense2_description:
|
||||
debian:
|
||||
buster: [ros-noetic-realsense2-description]
|
||||
|
||||
@@ -60,24 +60,4 @@ domain-needed
|
||||
quiet-dhcp6
|
||||
EOF
|
||||
|
||||
echo_stamp "#4 Build the RTL8814AU Wi-Fi adapter driver"
|
||||
cd /home/pi
|
||||
git clone https://github.com/aircrack-ng/rtl8812au.git --depth=1
|
||||
cd rtl8812au
|
||||
echo kernel version: $(uname -r)
|
||||
echo kernel version from procfs: $(cat /proc/version)
|
||||
echo version: $(git describe --tags --always)
|
||||
sed -i 's/CONFIG_PLATFORM_I386_PC = y/CONFIG_PLATFORM_I386_PC = n/g' Makefile # https://github.com/aircrack-ng/rtl8812au#for-raspberry-rpi
|
||||
sed -i 's/CONFIG_PLATFORM_ARM_RPI = n/CONFIG_PLATFORM_ARM_RPI = y/g' Makefile
|
||||
# sed -i 's/CONFIG_PLATFORM_ARM64_RPI = n/CONFIG_PLATFORM_ARM64_RPI = y/g' Makefile
|
||||
apt-cache policy raspberrypi-kernel-headers
|
||||
apt-get install -y raspberrypi-kernel-headers dkms
|
||||
ls /lib/modules
|
||||
KERNEL_VERSION=$(cd /lib/modules && echo *-v7l+) # can't use `uname` as it gives incorrect value in emulated environment
|
||||
echo make
|
||||
make KERNEL_VER=$KERNEL_VERSION KVER=$KERNEL_VERSION
|
||||
echo make install
|
||||
make install KERNEL_VER=$KERNEL_VERSION KVER=$KERNEL_VERSION
|
||||
# TODO: rm -rf /home/pi/rtl8812au
|
||||
|
||||
echo_stamp "#4 End of network installation"
|
||||
|
||||
@@ -13,7 +13,7 @@
|
||||
# copies or substantial portions of the Software.
|
||||
#
|
||||
|
||||
set -ex # exit on error, echo commands
|
||||
set -e # Exit immidiately on non-zero result
|
||||
|
||||
REPO=$1
|
||||
REF=$2
|
||||
@@ -49,7 +49,7 @@ echo_stamp() {
|
||||
my_travis_retry() {
|
||||
local result=0
|
||||
local count=1
|
||||
local max_count=5
|
||||
local max_count=50
|
||||
while [ $count -le $max_count ]; do
|
||||
[ $result -ne 0 ] && {
|
||||
echo -e "\nThe command \"$@\" failed. Retrying, $count of $max_count.\n" >&2
|
||||
@@ -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.1-0buster \
|
||||
ros-${ROS_DISTRO}-cv-camera=0.5.0-0buster \
|
||||
ros-${ROS_DISTRO}-image-publisher=1.15.3-0buster \
|
||||
ros-${ROS_DISTRO}-web-video-server=0.2.1-0buster
|
||||
apt-mark hold \
|
||||
@@ -112,7 +112,7 @@ my_travis_retry pip3 install wheel
|
||||
my_travis_retry pip3 install -r /home/pi/catkin_ws/src/clover/clover/requirements.txt
|
||||
source /opt/ros/${ROS_DISTRO}/setup.bash
|
||||
# Don't build simulation plugins for actual drone
|
||||
catkin_make -j2 -DCMAKE_BUILD_TYPE=RelWithDebInfo
|
||||
catkin_make -j2 -DCMAKE_BUILD_TYPE=RelWithDebInfo -DCATKIN_BLACKLIST_PACKAGES=clover_gazebo_plugins
|
||||
source devel/setup.bash
|
||||
|
||||
echo_stamp "Install clever package (for backwards compatibility)"
|
||||
@@ -137,8 +137,7 @@ my_travis_retry apt-get install -y --no-install-recommends \
|
||||
ros-${ROS_DISTRO}-ws281x \
|
||||
ros-${ROS_DISTRO}-rosshow \
|
||||
ros-${ROS_DISTRO}-cmake-modules \
|
||||
ros-${ROS_DISTRO}-image-view \
|
||||
ros-${ROS_DISTRO}-image-geometry
|
||||
ros-${ROS_DISTRO}-image-view
|
||||
|
||||
# TODO move GeographicLib datasets to Mavros debian package
|
||||
echo_stamp "Install GeographicLib datasets (needed for mavros)" \
|
||||
@@ -152,9 +151,6 @@ catkin_make run_tests #&& catkin_test_results
|
||||
echo_stamp "Change permissions for catkin_ws"
|
||||
chown -Rf pi:pi /home/pi/catkin_ws
|
||||
|
||||
echo_stamp "Update www"
|
||||
sudo -u pi sh -c ". devel/setup.sh && rosrun clover www"
|
||||
|
||||
echo_stamp "Make \$HOME/examples symlink"
|
||||
ln -s "$(catkin_find clover examples --first-only)" /home/pi
|
||||
chown -Rf pi:pi /home/pi/examples
|
||||
|
||||
@@ -137,8 +137,6 @@ pip3 --version
|
||||
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]
|
||||
|
||||
@@ -37,7 +37,3 @@ apt-cache show openvpn
|
||||
|
||||
echo "Move /etc/ld.so.preload back to its original position"
|
||||
mv /etc/ld.so.preload.disabled-for-build /etc/ld.so.preload
|
||||
|
||||
echo "Largest packages installed"
|
||||
sudo -E sh -c 'apt-get install -y debian-goodies'
|
||||
dpigs -H -n 100
|
||||
|
||||
@@ -2,7 +2,6 @@
|
||||
|
||||
# validate all required modules installed
|
||||
|
||||
import os
|
||||
import rospy
|
||||
from geometry_msgs.msg import PoseStamped
|
||||
from sensor_msgs.msg import Range, BatteryState
|
||||
@@ -23,7 +22,6 @@ from clover.srv import GetTelemetry, Navigate, NavigateGlobal, SetPosition, SetV
|
||||
from led_msgs.srv import SetLEDs
|
||||
from led_msgs.msg import LEDStateArray, LEDState
|
||||
from aruco_pose.msg import Marker, MarkerArray, Point2D
|
||||
from clover import long_callback
|
||||
|
||||
import dynamic_reconfigure.client
|
||||
|
||||
@@ -33,15 +31,9 @@ import tf2_geometry_msgs
|
||||
import VL53L1X
|
||||
import pymavlink
|
||||
from pymavlink import mavutil
|
||||
from image_geometry import PinholeCameraModel, StereoCameraModel
|
||||
import rpi_ws281x
|
||||
import pigpio
|
||||
# from espeak import espeak
|
||||
from pyzbar import pyzbar
|
||||
import docopt
|
||||
import geopy
|
||||
import flask
|
||||
|
||||
print(cv2.getBuildInformation())
|
||||
|
||||
if not os.environ.get('VM'):
|
||||
import rpi_ws281x
|
||||
import pigpio
|
||||
|
||||
@@ -6,10 +6,16 @@ set -ex
|
||||
|
||||
# validate required software is installed
|
||||
|
||||
python --version
|
||||
python2 --version
|
||||
python3 --version
|
||||
ipython --version
|
||||
ipython3 --version
|
||||
|
||||
# ptvsd does not have a stand-alone binary
|
||||
python -m ptvsd --version
|
||||
python3 -m ptvsd --version
|
||||
|
||||
node -v
|
||||
npm -v
|
||||
|
||||
@@ -19,77 +25,38 @@ lsof -v
|
||||
git --version
|
||||
vim --version
|
||||
pip --version
|
||||
pip2 --version
|
||||
pip3 --version
|
||||
tcpdump --version
|
||||
monkey --version
|
||||
pigpiod -v
|
||||
i2cdetect -V
|
||||
butterfly -h
|
||||
# espeak --version
|
||||
mjpg_streamer --version
|
||||
systemctl --version
|
||||
|
||||
if [ -z $VM ]; then
|
||||
# rpi only software
|
||||
python --version
|
||||
ipython --version
|
||||
pip2 --version
|
||||
# `python` is python2 for now
|
||||
[[ $(python -c 'import sys;print(sys.version_info.major)') == "2" ]]
|
||||
|
||||
# ptvsd does not have a stand-alone binary
|
||||
python -m ptvsd --version
|
||||
python3 -m ptvsd --version
|
||||
|
||||
pigpiod -v
|
||||
i2cdetect -V
|
||||
butterfly -h
|
||||
mjpg_streamer --version
|
||||
fi
|
||||
|
||||
# ros stuff
|
||||
|
||||
roscore -h
|
||||
rosversion clover
|
||||
rosversion aruco_pose
|
||||
rosversion vl53l1x
|
||||
rosversion mavros
|
||||
rosversion mavros_extras
|
||||
rosversion ws281x
|
||||
rosversion led_msgs
|
||||
rosversion dynamic_reconfigure
|
||||
rosversion tf2_web_republisher
|
||||
rosversion rosbridge_server
|
||||
rosversion compressed_image_transport
|
||||
rosversion rosbridge_suite
|
||||
rosversion rosserial
|
||||
rosversion usb_cam
|
||||
rosversion cv_camera
|
||||
rosversion web_video_server
|
||||
rosversion rosshow
|
||||
rosversion nodelet
|
||||
rosversion image_view
|
||||
|
||||
[[ $(rosversion ws281x) == "0.0.13" ]]
|
||||
|
||||
if [ -z $VM ]; then
|
||||
rosversion compressed_image_transport
|
||||
rosversion rosshow
|
||||
rosversion vl53l1x
|
||||
rosversion rosserial
|
||||
[[ $(rosversion cv_camera) == "0.5.1" ]] # patched version with init fix
|
||||
fi
|
||||
|
||||
# determine user home directory
|
||||
[ $VM ] && H="/home/clover" || H="/home/pi"
|
||||
|
||||
# test basic ros tool work
|
||||
source $H/catkin_ws/devel/setup.bash
|
||||
roscd
|
||||
rosrun
|
||||
rosmsg
|
||||
rossrv
|
||||
rosnode || [ $? -eq 64 ] # usage output code is 64
|
||||
rostopic || [ $? -eq 64 ]
|
||||
rosservice || [ $? -eq 64 ]
|
||||
rosparam
|
||||
roslaunch -h
|
||||
|
||||
# validate examples are present
|
||||
[[ $(ls $H/examples/*) ]]
|
||||
|
||||
# validate web tools present
|
||||
[ -d $H/.ros/www ]
|
||||
[ "$(readlink $H/.ros/www/clover)" = "$H/catkin_ws/src/clover/clover/www" ]
|
||||
[ "$(readlink $H/.ros/www/clover_blocks)" = "$H/catkin_ws/src/clover/clover_blocks/www" ]
|
||||
[[ $(ls /home/pi/examples/*) ]]
|
||||
|
||||
@@ -17,7 +17,6 @@ find_package(catkin REQUIRED COMPONENTS
|
||||
message_generation
|
||||
geometry_msgs
|
||||
sensor_msgs
|
||||
led_msgs
|
||||
geographic_msgs
|
||||
tf
|
||||
tf2
|
||||
@@ -25,7 +24,6 @@ find_package(catkin REQUIRED COMPONENTS
|
||||
tf2_ros
|
||||
image_transport
|
||||
cv_bridge
|
||||
dynamic_reconfigure
|
||||
)
|
||||
|
||||
list(APPEND CMAKE_MODULE_PATH "${CMAKE_CURRENT_LIST_DIR}/cmake")
|
||||
@@ -53,7 +51,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 ##
|
||||
@@ -80,10 +78,11 @@ catkin_python_setup()
|
||||
## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...)
|
||||
|
||||
## Generate messages in the 'msg' folder
|
||||
add_message_files(
|
||||
FILES
|
||||
State.msg
|
||||
)
|
||||
# add_message_files(
|
||||
# FILES
|
||||
# Message1.msg
|
||||
# Message2.msg
|
||||
# )
|
||||
|
||||
## Generate services in the 'srv' folder
|
||||
add_service_files(
|
||||
@@ -91,9 +90,6 @@ add_service_files(
|
||||
GetTelemetry.srv
|
||||
Navigate.srv
|
||||
NavigateGlobal.srv
|
||||
SetAltitude.srv
|
||||
SetYaw.srv
|
||||
SetYawRate.srv
|
||||
SetPosition.srv
|
||||
SetVelocity.srv
|
||||
SetAttitude.srv
|
||||
@@ -130,9 +126,10 @@ generate_messages(
|
||||
## and list every .cfg file to be processed
|
||||
|
||||
## Generate dynamic reconfigure parameters in the 'cfg' folder
|
||||
generate_dynamic_reconfigure_options(
|
||||
cfg/Flow.cfg
|
||||
)
|
||||
# generate_dynamic_reconfigure_options(
|
||||
# cfg/DynReconf1.cfg
|
||||
# cfg/DynReconf2.cfg
|
||||
# )
|
||||
|
||||
###################################
|
||||
## catkin specific configuration ##
|
||||
@@ -214,8 +211,6 @@ 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
|
||||
@@ -308,5 +303,4 @@ endif()
|
||||
if (CATKIN_ENABLE_TESTING)
|
||||
find_package(rostest REQUIRED)
|
||||
add_rostest(test/basic.test)
|
||||
add_rostest(test/offboard.test)
|
||||
endif()
|
||||
|
||||
@@ -1,10 +0,0 @@
|
||||
#!/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"))
|
||||
@@ -1,64 +0,0 @@
|
||||
# Information: https://clover.coex.tech/camera
|
||||
|
||||
# Example on basic working with the camera and image processing:
|
||||
|
||||
# - cuts out a central square from the camera image;
|
||||
# - publishes this cropped image to the topic `/cv/center`;
|
||||
# - computes the average color of it;
|
||||
# - prints its name to the console.
|
||||
|
||||
import rospy
|
||||
import cv2
|
||||
from sensor_msgs.msg import Image
|
||||
from cv_bridge import CvBridge
|
||||
from clover import long_callback
|
||||
|
||||
rospy.init_node('cv')
|
||||
bridge = CvBridge()
|
||||
|
||||
printed_color = None
|
||||
center_pub = rospy.Publisher('~center', Image, queue_size=1)
|
||||
|
||||
def get_color_name(h):
|
||||
if h < 15: return 'red'
|
||||
elif h < 30: return 'orange'
|
||||
elif h < 60: return 'yellow'
|
||||
elif h < 90: return 'green'
|
||||
elif h < 120: return 'cyan'
|
||||
elif h < 150: return 'blue'
|
||||
elif h < 170: return 'magenta'
|
||||
else: return 'red'
|
||||
|
||||
|
||||
@long_callback
|
||||
def image_callback(msg):
|
||||
img = bridge.imgmsg_to_cv2(msg, 'bgr8')
|
||||
|
||||
# convert to HSV to work with color hue
|
||||
img_hsv = cv2.cvtColor(img, cv2.COLOR_BGR2HSV)
|
||||
|
||||
# cut out a central square
|
||||
w = img.shape[1]
|
||||
h = img.shape[0]
|
||||
r = 20
|
||||
center = img_hsv[h // 2 - r:h // 2 + r, w // 2 - r:w // 2 + r]
|
||||
|
||||
# compute and print the average hue
|
||||
mean_hue = center[:, :, 0].mean()
|
||||
color = get_color_name(mean_hue)
|
||||
global printed_color
|
||||
if color != printed_color:
|
||||
print(color)
|
||||
printed_color = color
|
||||
|
||||
# publish the cropped image
|
||||
center = cv2.cvtColor(center, cv2.COLOR_HSV2BGR)
|
||||
center_pub.publish(bridge.cv2_to_imgmsg(center, 'bgr8'))
|
||||
|
||||
# process every frame:
|
||||
image_sub = rospy.Subscriber('main_camera/image_raw', Image, image_callback, queue_size=1)
|
||||
|
||||
# process 5 frames per second:
|
||||
# image_sub = rospy.Subscriber('main_camera/image_raw_throttled', Image, image_callback, queue_size=1)
|
||||
|
||||
rospy.spin()
|
||||
@@ -16,8 +16,11 @@ set_attitude = rospy.ServiceProxy('set_attitude', srv.SetAttitude)
|
||||
set_rates = rospy.ServiceProxy('set_rates', srv.SetRates)
|
||||
land = rospy.ServiceProxy('land', Trigger)
|
||||
|
||||
def navigate_wait(x=0, y=0, z=0, yaw=math.nan, speed=0.5, frame_id='body', tolerance=0.2, auto_arm=False):
|
||||
res = navigate(x=x, y=y, z=z, yaw=yaw, speed=speed, frame_id=frame_id, auto_arm=auto_arm)
|
||||
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
|
||||
|
||||
@@ -1,97 +0,0 @@
|
||||
# This example makes the drone find and follow the red circle.
|
||||
# To test in the simulator, place 'Red Circle' model on the floor.
|
||||
# More information: https://clover.coex.tech/red_circle
|
||||
|
||||
# Input topic: main_camera/image_raw (camera image)
|
||||
# Output topics:
|
||||
# cv/mask (red color mask)
|
||||
# cv/red_circle (position of the center of the red circle in 3D space)
|
||||
|
||||
import rospy
|
||||
import cv2
|
||||
import numpy as np
|
||||
from math import nan
|
||||
from sensor_msgs.msg import Image, CameraInfo
|
||||
from geometry_msgs.msg import PointStamped, Point
|
||||
from cv_bridge import CvBridge
|
||||
from clover import long_callback, srv
|
||||
import tf2_ros
|
||||
import tf2_geometry_msgs
|
||||
|
||||
rospy.init_node('cv', disable_signals=True) # disable signals to allow interrupting with ctrl+c
|
||||
|
||||
get_telemetry = rospy.ServiceProxy('get_telemetry', srv.GetTelemetry)
|
||||
set_position = rospy.ServiceProxy('set_position', srv.SetPosition)
|
||||
|
||||
bridge = CvBridge()
|
||||
|
||||
tf_buffer = tf2_ros.Buffer()
|
||||
tf_listener = tf2_ros.TransformListener(tf_buffer)
|
||||
|
||||
mask_pub = rospy.Publisher('~mask', Image, queue_size=1)
|
||||
point_pub = rospy.Publisher('~red_circle', PointStamped, queue_size=1)
|
||||
|
||||
# read camera info
|
||||
camera_info = rospy.wait_for_message('main_camera/camera_info', CameraInfo)
|
||||
camera_matrix = np.float64(camera_info.K).reshape(3, 3)
|
||||
distortion = np.float64(camera_info.D).flatten()
|
||||
|
||||
|
||||
def img_xy_to_point(xy, dist):
|
||||
xy = cv2.undistortPoints(xy, camera_matrix, distortion, P=camera_matrix)[0][0]
|
||||
|
||||
# Shift points to center
|
||||
xy -= camera_info.width // 2, camera_info.height // 2
|
||||
|
||||
fx = camera_matrix[0, 0]
|
||||
fy = camera_matrix[1, 1]
|
||||
|
||||
return Point(x=xy[0] * dist / fx, y=xy[1] * dist / fy, z=dist)
|
||||
|
||||
def get_center_of_mass(mask):
|
||||
M = cv2.moments(mask)
|
||||
if M['m00'] == 0:
|
||||
return None
|
||||
return M['m10'] // M['m00'], M['m01'] // M['m00']
|
||||
|
||||
follow_red_circle = False
|
||||
|
||||
@long_callback
|
||||
def image_callback(msg):
|
||||
img = bridge.imgmsg_to_cv2(msg, 'bgr8')
|
||||
img_hsv = cv2.cvtColor(img, cv2.COLOR_BGR2HSV)
|
||||
|
||||
# we need to use two ranges for red color
|
||||
mask1 = cv2.inRange(img_hsv, (0, 150, 150), (15, 255, 255))
|
||||
mask2 = cv2.inRange(img_hsv, (160, 150, 150), (180, 255, 255))
|
||||
|
||||
# combine two masks using bitwise OR
|
||||
mask = cv2.bitwise_or(mask1, mask2)
|
||||
|
||||
# publish the mask
|
||||
if mask_pub.get_num_connections() > 0:
|
||||
mask_pub.publish(bridge.cv2_to_imgmsg(mask, 'mono8'))
|
||||
|
||||
# calculate x and y of the circle
|
||||
xy = get_center_of_mass(mask)
|
||||
if xy is None:
|
||||
return
|
||||
|
||||
# calculate and publish the position of the circle in 3D space
|
||||
altitude = get_telemetry('terrain').z
|
||||
xy3d = img_xy_to_point(xy, altitude)
|
||||
target = PointStamped(header=msg.header, point=xy3d)
|
||||
point_pub.publish(target)
|
||||
|
||||
if follow_red_circle:
|
||||
# follow the target
|
||||
setpoint = tf_buffer.transform(target, 'map', timeout=rospy.Duration(0.2))
|
||||
set_position(x=setpoint.point.x, y=setpoint.point.y, z=nan, yaw=nan, frame_id=setpoint.header.frame_id)
|
||||
|
||||
# process each camera frame:
|
||||
image_sub = rospy.Subscriber('main_camera/image_raw', Image, image_callback, queue_size=1)
|
||||
|
||||
rospy.loginfo('Hit enter to follow the red circle')
|
||||
input()
|
||||
follow_red_circle = True
|
||||
rospy.spin()
|
||||
@@ -8,21 +8,16 @@
|
||||
|
||||
<!-- 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="$(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">
|
||||
<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">
|
||||
<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/map"/>
|
||||
<remap from="map_markers" to="aruco_map/markers"/>
|
||||
<param name="estimate_poses" value="true"/>
|
||||
<param name="send_tf" value="true"/>
|
||||
<param name="use_map_markers" value="true"/>
|
||||
<param name="known_vertical" value="map" if="$(eval placement == 'floor' or placement == 'ceiling')"/>
|
||||
<param name="flip_vertical" value="true" if="$(eval placement == 'ceiling')"/>
|
||||
<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 -->
|
||||
@@ -31,13 +26,13 @@
|
||||
</node>
|
||||
|
||||
<!-- aruco_map: estimate aruco map pose -->
|
||||
<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">
|
||||
<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">
|
||||
<remap from="image_raw" to="main_camera/image_raw"/>
|
||||
<remap from="camera_info" to="main_camera/camera_info"/>
|
||||
<remap from="markers" to="aruco_detect/markers"/>
|
||||
<param name="map" value="$(find aruco_pose)/map/$(arg map)"/>
|
||||
<param name="known_vertical" value="map" if="$(eval placement == 'floor' or placement == 'ceiling')"/>
|
||||
<param name="flip_vertical" value="true" if="$(eval placement == 'ceiling')"/>
|
||||
<param name="known_tilt" value="map" if="$(eval placement == 'floor')"/>
|
||||
<param name="known_tilt" value="map_flipped" if="$(eval placement == 'ceiling')"/>
|
||||
<param name="image_axis" value="true"/>
|
||||
<param name="frame_id" value="aruco_map_detected" if="$(arg aruco_vpe)"/>
|
||||
<param name="frame_id" value="aruco_map" unless="$(arg aruco_vpe)"/>
|
||||
@@ -46,11 +41,11 @@
|
||||
</node>
|
||||
|
||||
<!-- vpe publisher from aruco markers -->
|
||||
<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)"/>
|
||||
<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"/>
|
||||
<remap from="~vpe" to="mavros/vision_pose/pose"/>
|
||||
<param name="frame_id" value="aruco_map_detected" if="$(arg aruco_vpe)"/>
|
||||
<param name="force_init" value="$(arg force_init)"/>
|
||||
<param name="frame_id" value="aruco_map_detected"/>
|
||||
<param name="publish_zero" value="true"/>
|
||||
<param name="offset_frame_id" value="aruco_map"/>
|
||||
</node>
|
||||
</launch>
|
||||
|
||||
@@ -12,7 +12,6 @@
|
||||
<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 -->
|
||||
|
||||
@@ -34,10 +33,7 @@
|
||||
</node>
|
||||
|
||||
<!-- aruco markers -->
|
||||
<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>
|
||||
<include file="$(find clover)/launch/aruco.launch" if="$(arg aruco)"/>
|
||||
|
||||
<!-- optical flow -->
|
||||
<node pkg="nodelet" type="nodelet" name="optical_flow" args="load clover/optical_flow main_camera_nodelet_manager" if="$(arg optical_flow)" clear_params="true" output="screen" respawn="true">
|
||||
@@ -45,13 +41,16 @@
|
||||
<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="true"/>
|
||||
</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"/>
|
||||
<param name="terrain_frame_mode" value="range"/>
|
||||
</node>
|
||||
|
||||
<!-- main camera -->
|
||||
@@ -72,9 +71,6 @@
|
||||
<param name="pass_statuses" type="yaml" value="[0, 6, 7, 11]"/>
|
||||
</node>
|
||||
|
||||
<!-- rangefinder's frame -->
|
||||
<node pkg="tf2_ros" type="static_transform_publisher" name="rangefinder_frame" args="0 0 -0.05 0 1.5707963268 0 base_link rangefinder" if="$(arg rangefinder_vl53l1x)"/>
|
||||
|
||||
<!-- led strip -->
|
||||
<include file="$(find clover)/launch/led.launch" if="$(arg led)">
|
||||
<arg name="simulator" value="$(arg simulator)"/>
|
||||
@@ -89,4 +85,8 @@
|
||||
<param name="use_fake_gcs" value="false"/>
|
||||
</node>
|
||||
|
||||
<!-- Update static directory -->
|
||||
<node pkg="roswww_static" name="roswww_static" type="main.py" clear_params="true">
|
||||
<param name="default_package" value="clover"/>
|
||||
</node>
|
||||
</launch>
|
||||
|
||||
@@ -4,8 +4,6 @@
|
||||
<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"/>
|
||||
@@ -45,8 +43,4 @@
|
||||
<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>
|
||||
|
||||
@@ -1,5 +1,5 @@
|
||||
<launch>
|
||||
<arg name="fcu_conn" default="usb"/> <!-- options: usb, uart, tcp, udp, sitl, hitl -->
|
||||
<arg name="fcu_conn" default="usb"/> <!-- options: usb, uart, tcp, udp, sitl -->
|
||||
<arg name="fcu_ip" default="127.0.0.1"/>
|
||||
<arg name="fcu_sys_id" default="1"/>
|
||||
<arg name="gcs_bridge" default="tcp"/>
|
||||
@@ -23,9 +23,6 @@
|
||||
<!-- 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)" />
|
||||
|
||||
@@ -42,7 +39,7 @@
|
||||
<rosparam command="load" file="$(find clover)/launch/mavros_config.yaml"/>
|
||||
|
||||
<!-- remap rangefinder -->
|
||||
<remap from="mavros/distance_sensor/rangefinder_sub" to="$(arg distance_sensor_remap)" if="$(eval bool(distance_sensor_remap))"/>
|
||||
<remap from="mavros/distance_sensor/rangefinder_sub" to="rangefinder/range"/>
|
||||
|
||||
<rosparam param="plugin_whitelist">
|
||||
- altitude
|
||||
@@ -77,6 +74,9 @@
|
||||
covariance: 1 # cm
|
||||
</rosparam>
|
||||
|
||||
<!-- Rangefinders frame -->
|
||||
<node pkg="tf2_ros" type="static_transform_publisher" name="rangefinder_frame" args="0 0 -0.05 0 1.5707963268 0 base_link rangefinder"/>
|
||||
|
||||
<!-- Copter visualization -->
|
||||
<node name="visualization" pkg="mavros_extras" type="visualization" if="$(arg viz)">
|
||||
<remap to="mavros/local_position/pose" from="local_position"/>
|
||||
|
||||
@@ -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: false
|
||||
startup_px4_usb_quirk: true
|
||||
|
||||
conn:
|
||||
heartbeat_rate: 1.0 # send heartbeat rate in Hertz
|
||||
timeout: 10.0 # heartbeat timeout in seconds
|
||||
heartbeat_rate: 1.0 # send hertbeat rate in Hertz
|
||||
timeout: 10.0 # hertbeat 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)
|
||||
|
||||
@@ -13,7 +13,6 @@ time:
|
||||
time_ref_source: "fcu" # time_reference source
|
||||
timesync_mode: MAVLINK
|
||||
timesync_avg_alpha: 0.6 # timesync averaging factor
|
||||
publish_sim_time: false # don't publish /clock
|
||||
|
||||
global_position:
|
||||
frame_id: "map" # origin frame
|
||||
@@ -78,9 +77,6 @@ distance_sensor:
|
||||
field_of_view: 0.5
|
||||
rangefinder_sub:
|
||||
subscriber: true
|
||||
id: 1
|
||||
orientation: PITCH_270
|
||||
covariance: 1 # cm
|
||||
|
||||
# fake_gps
|
||||
fake_gps:
|
||||
|
||||
@@ -1,4 +0,0 @@
|
||||
<launch>
|
||||
<!-- shortcut for running the simulation (`roslaunch clover simulator.launch`) -->
|
||||
<include file="$(find clover_simulation)/launch/simulator.launch"/>
|
||||
</launch>
|
||||
@@ -1,38 +0,0 @@
|
||||
uint8 MODE_NONE = 0
|
||||
uint8 MODE_NAVIGATE = 1
|
||||
uint8 MODE_NAVIGATE_GLOBAL = 2
|
||||
uint8 MODE_POSITION = 3
|
||||
uint8 MODE_VELOCITY = 4
|
||||
uint8 MODE_ATTITUDE = 5
|
||||
uint8 MODE_RATES = 6
|
||||
|
||||
uint8 YAW_MODE_YAW = 0
|
||||
uint8 YAW_MODE_YAW_RATE = 1
|
||||
uint8 YAW_MODE_YAW_TOWARDS = 2
|
||||
|
||||
# type of offboard control
|
||||
uint8 mode
|
||||
uint8 yaw_mode
|
||||
|
||||
# targets
|
||||
float32 x
|
||||
float32 y
|
||||
float32 z
|
||||
float32 speed
|
||||
float32 lat
|
||||
float32 lon
|
||||
float32 vx
|
||||
float32 vy
|
||||
float32 vz
|
||||
float32 roll
|
||||
float32 pitch
|
||||
float32 yaw
|
||||
float32 roll_rate
|
||||
float32 pitch_rate
|
||||
float32 yaw_rate
|
||||
float32 thrust
|
||||
|
||||
# frames of reference
|
||||
string xy_frame_id
|
||||
string z_frame_id
|
||||
string yaw_frame_id
|
||||
@@ -1,7 +1,7 @@
|
||||
<?xml version="1.0"?>
|
||||
<package format="3">
|
||||
<name>clover</name>
|
||||
<version>0.24.0</version>
|
||||
<version>0.21.1</version>
|
||||
<description>The Clover package</description>
|
||||
|
||||
<maintainer email="okalachev@gmail.com">Oleg Kalachev</maintainer>
|
||||
@@ -37,13 +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>
|
||||
<test_depend>ros_pytest</test_depend>
|
||||
<!-- Use test_depend for packages you need only for testing: -->
|
||||
<!-- <test_depend>gtest</test_depend> -->
|
||||
|
||||
<!-- The export tag contains other, unspecified, tags -->
|
||||
<export>
|
||||
|
||||
@@ -1,4 +1,5 @@
|
||||
flask
|
||||
geopy
|
||||
smbus2
|
||||
VL53L1X
|
||||
flask==1.1.1
|
||||
docopt==0.6.2
|
||||
geopy==1.11.0
|
||||
smbus2==0.3.0
|
||||
VL53L1X==0.0.5
|
||||
|
||||
@@ -1,11 +0,0 @@
|
||||
## ! DO NOT MANUALLY INVOKE THIS setup.py, USE CATKIN INSTEAD
|
||||
|
||||
from distutils.core import setup
|
||||
from catkin_pkg.python_setup import generate_distutils_setup
|
||||
|
||||
# fetch values from package.xml
|
||||
setup_args = generate_distutils_setup(
|
||||
packages=['clover'],
|
||||
package_dir={'': 'src'})
|
||||
|
||||
setup(**setup_args)
|
||||
@@ -1,90 +0,0 @@
|
||||
#!/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
|
||||
|
||||
try:
|
||||
flow_client = dynamic_reconfigure.client.Client('optical_flow', timeout=2)
|
||||
except rospy.ROSException:
|
||||
flow_client = None
|
||||
print('Cannot configure optical flow, skip')
|
||||
|
||||
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=math.nan, speed=0.5, frame_id='body', tolerance=0.2, auto_arm=False):
|
||||
res = navigate(x=x, y=y, z=z, yaw=yaw, 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()
|
||||
|
||||
if flow_client:
|
||||
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()
|
||||
@@ -1,99 +0,0 @@
|
||||
#!/usr/bin/env python3
|
||||
|
||||
import rospy
|
||||
import math
|
||||
from math import nan, inf
|
||||
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_yaw = handle_response(rospy.ServiceProxy('set_yaw', srv.SetYaw))
|
||||
set_yaw_rate = handle_response(rospy.ServiceProxy('set_yaw_rate', srv.SetYawRate))
|
||||
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, speed=0.5, frame_id='body', tolerance=0.2, auto_arm=False):
|
||||
res = navigate(x=x, y=y, z=z, yaw=yaw, 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° using set_yaw [enter] ')
|
||||
set_yaw(yaw=-math.pi / 2, frame_id='navigate_target')
|
||||
rospy.sleep(3)
|
||||
|
||||
input('Use set_attitude to fly backwards [enter]')
|
||||
set_attitude(roll=0, pitch=-0.3, 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(roll=0.3, pitch=0, 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 set_yaw_rate [enter]')
|
||||
set_yaw_rate(yaw_rate=-1)
|
||||
rospy.sleep(2 * math.pi)
|
||||
set_position(frame_id='body')
|
||||
|
||||
input('Return to start point heading forward [enter]')
|
||||
navigate_wait(x=start.x, y=start.y, z=start.z, yaw=inf, speed=1, frame_id='map')
|
||||
|
||||
input('Land [enter]')
|
||||
land()
|
||||
@@ -1,72 +0,0 @@
|
||||
#!/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()
|
||||
@@ -1,11 +0,0 @@
|
||||
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
|
||||
@@ -1,35 +0,0 @@
|
||||
import rospy
|
||||
from threading import Thread, Event
|
||||
|
||||
def long_callback(fn):
|
||||
"""
|
||||
Decorator fixing a rospy issue for long-running topic callbacks, primarily
|
||||
for image processing.
|
||||
|
||||
See: https://github.com/ros/ros_comm/issues/1901.
|
||||
|
||||
Usage example:
|
||||
|
||||
@long_callback
|
||||
def image_callback(msg):
|
||||
# perform image processing
|
||||
# ...
|
||||
|
||||
rospy.Subscriber('main_camera/image_raw', Image, image_callback, queue_size=1)
|
||||
"""
|
||||
e = Event()
|
||||
|
||||
def thread():
|
||||
while not rospy.is_shutdown():
|
||||
e.wait()
|
||||
e.clear()
|
||||
fn(thread.current_msg)
|
||||
|
||||
thread.current_msg = None
|
||||
Thread(target=thread, daemon=True).start()
|
||||
|
||||
def wrapper(msg):
|
||||
thread.current_msg = msg
|
||||
e.set()
|
||||
|
||||
return wrapper
|
||||
@@ -31,6 +31,7 @@ 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;
|
||||
@@ -86,8 +87,9 @@ void proceed(const ros::TimerEvent& event)
|
||||
set_leds.request.leds.resize(led_count);
|
||||
|
||||
if (current_effect.effect == "blink" || current_effect.effect == "blink_fast") {
|
||||
// enable on odd counter
|
||||
if (counter % 2 != 0) {
|
||||
blink_state = !blink_state;
|
||||
// toggle all leds
|
||||
if (blink_state) {
|
||||
fill(current_effect.r, current_effect.g, current_effect.b);
|
||||
} else {
|
||||
fill(0, 0, 0);
|
||||
@@ -220,7 +222,6 @@ 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;
|
||||
}
|
||||
@@ -319,8 +320,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);
|
||||
|
||||
@@ -22,14 +22,11 @@
|
||||
#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;
|
||||
|
||||
@@ -41,7 +38,6 @@ public:
|
||||
{}
|
||||
|
||||
private:
|
||||
bool enabled_;
|
||||
ros::Publisher flow_pub_, velo_pub_, shift_pub_;
|
||||
ros::Time prev_stamp_;
|
||||
std::string fcu_frame_id_, local_frame_id_;
|
||||
@@ -57,11 +53,6 @@ private:
|
||||
std::unique_ptr<tf2_ros::Buffer> tf_buffer_;
|
||||
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()
|
||||
{
|
||||
@@ -78,7 +69,6 @@ private:
|
||||
roi_px_ = nh_priv.param("roi", 128);
|
||||
roi_rad_ = nh_priv.param("roi_rad", 0.0);
|
||||
calc_flow_gyro_ = nh_priv.param("calc_flow_gyro", false);
|
||||
flow_gyro_default_ = nh_priv.param("flow_gyro_default", NAN);
|
||||
|
||||
img_pub_ = it_priv.advertise("debug", 1);
|
||||
flow_pub_ = nh.advertise<mavros_msgs::OpticalFlowRad>("mavros/px4flow/raw/send", 1);
|
||||
@@ -91,17 +81,6 @@ private:
|
||||
|
||||
img_sub_ = it.subscribeCamera("image_raw", 1, &OpticalFlow::flow, this);
|
||||
|
||||
disable_on_vpe_ = nh_priv.param("disable_on_vpe", false);
|
||||
if (disable_on_vpe_) {
|
||||
vpe_sub_ = nh.subscribe("mavros/vision_pose/pose", 1, &OpticalFlow::vpeCallback, this);
|
||||
}
|
||||
|
||||
dyn_srv_ = std::make_shared<dynamic_reconfigure::Server<clover::FlowConfig>>(nh_priv);
|
||||
dynamic_reconfigure::Server<clover::FlowConfig>::CallbackType cb;
|
||||
|
||||
cb = std::bind(&OpticalFlow::paramCallback, this, std::placeholders::_1, std::placeholders::_2);
|
||||
dyn_srv_->setCallback(cb);
|
||||
|
||||
NODELET_INFO("Optical Flow initialized");
|
||||
}
|
||||
|
||||
@@ -128,14 +107,6 @@ 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;
|
||||
@@ -169,7 +140,7 @@ private:
|
||||
|
||||
img.convertTo(curr_, CV_32F);
|
||||
|
||||
if (prev_.empty() || (msg->header.stamp - prev_stamp_).toSec() > 0.1) { // outdated previous frame
|
||||
if (prev_.empty()) {
|
||||
prev_ = curr_.clone();
|
||||
prev_stamp_ = msg->header.stamp;
|
||||
cv::createHanningWindow(hann_, curr_.size(), CV_32F);
|
||||
@@ -223,9 +194,9 @@ private:
|
||||
uint32_t integration_time_us = integration_time.toSec() * 1.0e6;
|
||||
|
||||
// Calculate flow gyro
|
||||
flow_.integrated_xgyro = flow_gyro_default_;
|
||||
flow_.integrated_ygyro = flow_gyro_default_;
|
||||
flow_.integrated_zgyro = flow_gyro_default_;
|
||||
flow_.integrated_xgyro = NAN;
|
||||
flow_.integrated_ygyro = NAN;
|
||||
flow_.integrated_zgyro = NAN;
|
||||
|
||||
if (calc_flow_gyro_) {
|
||||
try {
|
||||
@@ -251,14 +222,6 @@ private:
|
||||
prev_ = curr_.clone();
|
||||
prev_stamp_ = msg->header.stamp;
|
||||
|
||||
// Publish estimated angular velocity
|
||||
geometry_msgs::TwistStamped velo;
|
||||
velo.header.stamp = msg->header.stamp;
|
||||
velo.header.frame_id = fcu_frame_id_;
|
||||
velo.twist.angular.x = flow_fcu.vector.x / integration_time.toSec();
|
||||
velo.twist.angular.y = flow_fcu.vector.y / integration_time.toSec();
|
||||
velo_pub_.publish(velo);
|
||||
|
||||
publish_debug:
|
||||
// Publish debug image
|
||||
if (img_pub_.getNumSubscribers() > 0) {
|
||||
@@ -271,6 +234,14 @@ publish_debug:
|
||||
out_msg.image = img;
|
||||
img_pub_.publish(out_msg.toImageMsg());
|
||||
}
|
||||
|
||||
// Publish estimated angular velocity
|
||||
geometry_msgs::TwistStamped velo;
|
||||
velo.header.stamp = msg->header.stamp;
|
||||
velo.header.frame_id = fcu_frame_id_;
|
||||
velo.twist.angular.x = flow_fcu.vector.x / integration_time.toSec();
|
||||
velo.twist.angular.y = flow_fcu.vector.y / integration_time.toSec();
|
||||
velo_pub_.publish(velo);
|
||||
}
|
||||
}
|
||||
|
||||
@@ -291,18 +262,6 @@ 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)
|
||||
|
||||
@@ -91,7 +91,7 @@ private:
|
||||
void fakeGCSThread()
|
||||
{
|
||||
// Awful workaround for fixing PX4 not sending STATUSTEXTs
|
||||
// if there is no GCS heartbeats.
|
||||
// if there is no GCS hearbeats.
|
||||
// TODO: use timer
|
||||
// TODO: remove, when PX4 get this fixed.
|
||||
ros::Publisher mavlink_pub = nh.advertise<mavros_msgs::Mavlink>("mavlink/to", 1);
|
||||
|
||||
@@ -9,14 +9,13 @@
|
||||
# The above copyright notice and this permission notice shall be included in all
|
||||
# copies or substantial portions of the Software.
|
||||
|
||||
import os, sys
|
||||
import os
|
||||
import math
|
||||
import subprocess
|
||||
import re
|
||||
from collections import OrderedDict
|
||||
import traceback
|
||||
import threading
|
||||
from threading import Event, Thread, Lock
|
||||
from threading import Event
|
||||
import numpy
|
||||
import rospy
|
||||
import tf2_ros
|
||||
@@ -28,86 +27,67 @@ from mavros_msgs.msg import State, OpticalFlowRad, Mavlink
|
||||
from mavros_msgs.srv import ParamGet
|
||||
from geometry_msgs.msg import PoseStamped, TwistStamped, PoseWithCovarianceStamped, Vector3Stamped
|
||||
from visualization_msgs.msg import MarkerArray as VisualizationMarkerArray
|
||||
from diagnostic_msgs.msg import DiagnosticArray
|
||||
import tf.transformations as t
|
||||
from aruco_pose.msg import MarkerArray
|
||||
from mavros import mavlink
|
||||
import locale
|
||||
|
||||
|
||||
# TODO: check attitude is present
|
||||
# TODO: disk free space
|
||||
# TODO: map, base_link, body
|
||||
# TODO: rc service
|
||||
# TODO: perform commander check, ekf2 status on PX4
|
||||
# TODO: check if FCU params setter succeed
|
||||
# TODO: selfcheck ROS service (with blacklists for checks)
|
||||
|
||||
|
||||
rospy.init_node('selfcheck')
|
||||
|
||||
os.environ['ROSCONSOLE_FORMAT']='${message}'
|
||||
|
||||
# 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)
|
||||
|
||||
|
||||
thread_local = threading.local()
|
||||
reports_lock = Lock()
|
||||
|
||||
|
||||
# formatting colors
|
||||
if sys.stdout.isatty():
|
||||
GREY = '\033[90m'
|
||||
GREEN = '\033[92m'
|
||||
RED = '\033[31m'
|
||||
END = '\033[0m'
|
||||
else:
|
||||
GREY = GREEN = RED = END = ''
|
||||
failures = []
|
||||
infos = []
|
||||
current_check = None
|
||||
|
||||
|
||||
def failure(text, *args):
|
||||
msg = text % args
|
||||
thread_local.reports += [{'failure': msg}]
|
||||
rospy.logwarn('%s: %s', current_check, msg)
|
||||
failures.append(msg)
|
||||
|
||||
|
||||
def info(text, *args):
|
||||
msg = text % args
|
||||
thread_local.reports += [{'info': msg}]
|
||||
rospy.loginfo('%s: %s', current_check, msg)
|
||||
infos.append(msg)
|
||||
|
||||
|
||||
def check(name):
|
||||
def inner(fn):
|
||||
def wrapper(*args, **kwargs):
|
||||
start = rospy.get_time()
|
||||
thread_local.reports = []
|
||||
failures[:] = []
|
||||
infos[:] = []
|
||||
global current_check
|
||||
current_check = name
|
||||
try:
|
||||
fn(*args, **kwargs)
|
||||
except Exception as e:
|
||||
traceback.print_exc()
|
||||
rospy.logerr('%s: exception occurred', name)
|
||||
with reports_lock:
|
||||
for report in thread_local.reports:
|
||||
if 'failure' in report:
|
||||
rospy.logerr('%s: %s', name, report['failure'])
|
||||
elif 'info' in report:
|
||||
rospy.loginfo(GREY + name + END + ': ' + report['info'])
|
||||
if not thread_local.reports:
|
||||
rospy.loginfo(GREY + name + END + ': ' + GREEN + 'OK' + END)
|
||||
if rospy.get_param('~time', False):
|
||||
rospy.loginfo('%s: %.1f sec', name, rospy.get_time() - start)
|
||||
return
|
||||
if not failures and not infos:
|
||||
rospy.loginfo('%s: OK', name)
|
||||
return wrapper
|
||||
return inner
|
||||
|
||||
|
||||
def ff(value, precision=2):
|
||||
# safely format float or int
|
||||
if value is None:
|
||||
return RED + '???' + END
|
||||
if isinstance(value, float):
|
||||
return ('{:.' + str(precision + 1) + '}').format(value)
|
||||
elif isinstance(value, int):
|
||||
return str(value)
|
||||
|
||||
|
||||
param_get = rospy.ServiceProxy('mavros/param/get', ParamGet)
|
||||
|
||||
|
||||
def get_param(name, default=None):
|
||||
def get_param(name):
|
||||
try:
|
||||
res = param_get(param_id=name)
|
||||
except rospy.ServiceException as e:
|
||||
@@ -116,17 +96,12 @@ def get_param(name, default=None):
|
||||
|
||||
if not res.success:
|
||||
failure('unable to retrieve PX4 parameter %s', name)
|
||||
return default
|
||||
else:
|
||||
if res.value.integer != 0:
|
||||
return res.value.integer
|
||||
return res.value.real
|
||||
|
||||
|
||||
def get_paramf(name, precision=2):
|
||||
return ff(get_param(name), precision)
|
||||
|
||||
|
||||
recv_event = Event()
|
||||
link = mavutil.mavlink.MAVLink('', 255, 1)
|
||||
mavlink_pub = rospy.Publisher('mavlink/to', Mavlink, queue_size=1)
|
||||
@@ -171,24 +146,6 @@ def mavlink_exec(cmd, timeout=3.0):
|
||||
return mavlink_recv
|
||||
|
||||
|
||||
def read_diagnostics(name, key):
|
||||
e = Event()
|
||||
def cb(msg):
|
||||
for status in msg.status:
|
||||
if status.name.lower() == name.lower():
|
||||
for value in status.values:
|
||||
if value.key.lower() == key.lower():
|
||||
cb.value = value.value
|
||||
e.set()
|
||||
return
|
||||
|
||||
cb.value = None
|
||||
sub = rospy.Subscriber('/diagnostics', DiagnosticArray, cb)
|
||||
e.wait(1.0) # wait to read all the diagnostics from nodes publishing them
|
||||
sub.unregister()
|
||||
return cb.value
|
||||
|
||||
|
||||
BOARD_ROTATIONS = {
|
||||
0: 'no rotation',
|
||||
1: 'yaw 45°',
|
||||
@@ -234,31 +191,26 @@ def check_fcu():
|
||||
state = rospy.wait_for_message('mavros/state', State, timeout=3)
|
||||
if not state.connected:
|
||||
failure('no connection to the FCU (check wiring)')
|
||||
info('fcu_url = %s', rospy.get_param('mavros/fcu_url', '?'))
|
||||
return
|
||||
|
||||
if not is_process_running('px4', exact=True): # can't use px4 console in SITL
|
||||
clover_tag = re.compile(r'-cl[oe]ver\.\d+$')
|
||||
clover_fw = False
|
||||
# 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')
|
||||
|
||||
# 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')
|
||||
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
|
||||
|
||||
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')
|
||||
if not is_clover_firmware:
|
||||
failure('not running Clover PX4 firmware, https://clover.coex.tech/firmware')
|
||||
|
||||
est = get_param('SYS_MC_EST_GROUP')
|
||||
if est == 1:
|
||||
@@ -295,29 +247,21 @@ def check_fcu():
|
||||
if cbrk_usb_chk != 197848:
|
||||
failure('set parameter CBRK_USB_CHK to 197848 for flying with USB connected')
|
||||
|
||||
if not is_process_running('px4', exact=True): # skip battery check in SITL
|
||||
try:
|
||||
battery = rospy.wait_for_message('mavros/battery', BatteryState, timeout=3)
|
||||
if not battery.cell_voltage:
|
||||
failure('cell voltage is not available, https://clover.coex.tech/power')
|
||||
else:
|
||||
cell = battery.cell_voltage[0]
|
||||
if cell > 4.3 or cell < 3.0:
|
||||
failure('incorrect cell voltage: %.2f V, https://clover.coex.tech/power', cell)
|
||||
elif cell < 3.7:
|
||||
failure('critically low cell voltage: %.2f V, recharge battery', cell)
|
||||
except rospy.ROSException:
|
||||
failure('no battery state')
|
||||
|
||||
# time sync check
|
||||
try:
|
||||
info('time sync offset: %.2f s', float(read_diagnostics('mavros: Time Sync', 'Estimated time offset (s)')))
|
||||
except:
|
||||
failure('cannot read time sync offset')
|
||||
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')
|
||||
|
||||
except rospy.ROSException:
|
||||
failure('no MAVROS state (check wiring)')
|
||||
info('fcu_url = %s', rospy.get_param('mavros/fcu_url', '?'))
|
||||
|
||||
|
||||
def describe_direction(v):
|
||||
@@ -384,7 +328,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 command line (including arguments) to match
|
||||
args.append('-f') # use full process name to match
|
||||
args.append(binary)
|
||||
subprocess.check_output(args)
|
||||
return True
|
||||
@@ -394,24 +338,19 @@ 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', '?'))
|
||||
info('aruco_detect/length = %g m', rospy.get_param('aruco_detect/length'))
|
||||
except KeyError:
|
||||
failure('aruco_detect/length parameter is not set')
|
||||
known_vertical = rospy.get_param('aruco_detect/known_vertical', '')
|
||||
flip_vertical = rospy.get_param('aruco_detect/flip_vertical', False)
|
||||
description = ''
|
||||
if known_vertical == 'map' and not flip_vertical:
|
||||
description = ' (all markers are on the floor)'
|
||||
elif known_vertical == 'map' and flip_vertical:
|
||||
description = ' (all markers are on the ceiling)'
|
||||
info('aruco_detect/known_vertical = %s', known_vertical)
|
||||
info('aruco_detect/flip_vertical = %s%s', flip_vertical, description)
|
||||
known_tilt = rospy.get_param('aruco_detect/known_tilt', '')
|
||||
if known_tilt == 'map':
|
||||
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)
|
||||
try:
|
||||
markers = rospy.wait_for_message('aruco_detect/markers', MarkerArray, timeout=0.8)
|
||||
rospy.wait_for_message('aruco_detect/markers', MarkerArray, timeout=1)
|
||||
except rospy.ROSException:
|
||||
failure('no markers detection')
|
||||
return
|
||||
@@ -420,61 +359,42 @@ def check_aruco():
|
||||
return
|
||||
|
||||
if is_process_running('aruco_map', full=True):
|
||||
known_vertical = rospy.get_param('aruco_map/known_vertical', '')
|
||||
flip_vertical = rospy.get_param('aruco_map/flip_vertical', False)
|
||||
description = ''
|
||||
if known_vertical == 'map' and not flip_vertical:
|
||||
description += ' (markers map is on the floor)'
|
||||
elif known_vertical == 'map' and flip_vertical:
|
||||
description += ' (markers map is on the ceiling)'
|
||||
info('aruco_map/known_vertical = %s', known_vertical)
|
||||
info('aruco_map/flip_vertical = %s%s', flip_vertical, description)
|
||||
known_tilt = rospy.get_param('aruco_map/known_tilt', '')
|
||||
if known_tilt == 'map':
|
||||
known_tilt += ' (marker\'s map is on the floor)'
|
||||
elif known_tilt == 'map_flipped':
|
||||
known_tilt += ' (marker\'s map is on the ceiling)'
|
||||
info('aruco_map/known_tilt = %s', known_tilt)
|
||||
|
||||
try:
|
||||
visualization = rospy.wait_for_message('aruco_map/visualization', VisualizationMarkerArray, timeout=0.8)
|
||||
visualization = rospy.wait_for_message('aruco_map/visualization', VisualizationMarkerArray, timeout=1)
|
||||
info('map has %s markers', len(visualization.markers))
|
||||
except:
|
||||
failure('cannot read aruco_map/visualization topic')
|
||||
|
||||
try:
|
||||
rospy.wait_for_message('aruco_map/pose', PoseWithCovarianceStamped, timeout=0.8)
|
||||
rospy.wait_for_message('aruco_map/pose', PoseWithCovarianceStamped, timeout=1)
|
||||
except rospy.ROSException:
|
||||
if not markers:
|
||||
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')
|
||||
failure('no map detection')
|
||||
else:
|
||||
info('aruco_map is not running')
|
||||
|
||||
|
||||
def is_on_the_floor():
|
||||
try:
|
||||
dist = rospy.wait_for_message('rangefinder/range', Range, timeout=1)
|
||||
return dist.range < 0.3
|
||||
except rospy.ROSException:
|
||||
return False
|
||||
|
||||
|
||||
@check('Vision position estimate')
|
||||
def check_vpe():
|
||||
vis = None
|
||||
try:
|
||||
vis = rospy.wait_for_message('mavros/vision_pose/pose', PoseStamped, timeout=0.8)
|
||||
vis = rospy.wait_for_message('mavros/vision_pose/pose', PoseStamped, timeout=1)
|
||||
except rospy.ROSException:
|
||||
try:
|
||||
vis = rospy.wait_for_message('mavros/mocap/pose', PoseStamped, timeout=0.8)
|
||||
vis = rospy.wait_for_message('mavros/mocap/pose', PoseStamped, timeout=1)
|
||||
except rospy.ROSException:
|
||||
if not is_process_running('vpe_publisher', full=True):
|
||||
info('no vision position estimate, vpe_publisher is not running')
|
||||
elif rospy.get_param('aruco_map/known_vertical', '') == 'map' \
|
||||
and rospy.get_param('aruco_map/flip_vertical', False):
|
||||
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')
|
||||
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
|
||||
|
||||
# check PX4 settings
|
||||
est = get_param('SYS_MC_EST_GROUP')
|
||||
@@ -486,14 +406,14 @@ def check_vpe():
|
||||
if vision_yaw_w == 0:
|
||||
failure('vision yaw weight is zero, change ATT_W_EXT_HDG parameter')
|
||||
else:
|
||||
info('vision yaw weight: %s', ff(vision_yaw_w))
|
||||
info('Vision yaw weight: %.2f', vision_yaw_w)
|
||||
fuse = get_param('LPE_FUSION')
|
||||
if not fuse & (1 << 2):
|
||||
failure('vision position fusion is disabled, change LPE_FUSION parameter')
|
||||
delay = get_param('LPE_VIS_DELAY')
|
||||
if delay != 0:
|
||||
failure('LPE_VIS_DELAY = %s, but it should be zero', delay)
|
||||
info('LPE_VIS_XY = %s m, LPE_VIS_Z = %s m', get_paramf('LPE_VIS_XY'), get_paramf('LPE_VIS_Z'))
|
||||
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'))
|
||||
elif est == 2:
|
||||
fuse = get_param('EKF2_AID_MASK')
|
||||
if not fuse & (1 << 3):
|
||||
@@ -502,10 +422,10 @@ def check_vpe():
|
||||
failure('vision yaw fusion is disabled, change EKF2_AID_MASK parameter')
|
||||
delay = get_param('EKF2_EV_DELAY')
|
||||
if delay != 0:
|
||||
failure('EKF2_EV_DELAY = %.2f, but it should be zero', delay)
|
||||
info('EKF2_EVA_NOISE = %s, EKF2_EVP_NOISE = %s',
|
||||
get_paramf('EKF2_EVA_NOISE', 3),
|
||||
get_paramf('EKF2_EVP_NOISE', 3))
|
||||
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'))
|
||||
|
||||
if not vis:
|
||||
return
|
||||
@@ -563,12 +483,6 @@ 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')
|
||||
|
||||
@@ -603,19 +517,13 @@ def check_velocity():
|
||||
@check('Global position (GPS)')
|
||||
def check_global_position():
|
||||
try:
|
||||
rospy.wait_for_message('mavros/global_position/global', NavSatFix, timeout=0.8)
|
||||
rospy.wait_for_message('mavros/global_position/global', NavSatFix, timeout=1)
|
||||
except rospy.ROSException:
|
||||
info('no global position')
|
||||
if get_param('SYS_MC_EST_GROUP') == 2 and (get_param('EKF2_AID_MASK', 0) & (1 << 0)):
|
||||
failure('enabled GPS fusion may suppress vision position aiding')
|
||||
|
||||
|
||||
@check('Optical flow')
|
||||
def check_optical_flow():
|
||||
if not is_process_running('optical_flow', full=True):
|
||||
info('optical_flow is not running')
|
||||
return
|
||||
|
||||
# TODO:check FPS!
|
||||
try:
|
||||
rospy.wait_for_message('mavros/px4flow/raw/send', OpticalFlowRad, timeout=0.5)
|
||||
@@ -623,7 +531,7 @@ def check_optical_flow():
|
||||
# check PX4 settings
|
||||
rot = get_param('SENS_FLOW_ROT')
|
||||
if rot != 0:
|
||||
failure('SENS_FLOW_ROT = %s, but it should be zero', rot)
|
||||
failure('SENS_FLOW_ROT parameter is %s, but it should be zero', rot)
|
||||
est = get_param('SYS_MC_EST_GROUP')
|
||||
if est == 1:
|
||||
fuse = get_param('LPE_FUSION')
|
||||
@@ -631,36 +539,32 @@ def check_optical_flow():
|
||||
failure('optical flow fusion is disabled, change LPE_FUSION parameter')
|
||||
if not fuse & (1 << 1):
|
||||
failure('flow gyro compensation is disabled, change LPE_FUSION parameter')
|
||||
scale = get_param('LPE_FLW_SCALE', 1)
|
||||
scale = get_param('LPE_FLW_SCALE')
|
||||
if not numpy.isclose(scale, 1.0):
|
||||
failure('LPE_FLW_SCALE = %.2f, but it should be 1.0', scale)
|
||||
failure('LPE_FLW_SCALE parameter is %.2f, but it should be 1.0', scale)
|
||||
|
||||
info('LPE_FLW_QMIN = %s, LPE_FLW_R = %s, LPE_FLW_RR = %s',
|
||||
get_paramf('LPE_FLW_QMIN'),
|
||||
get_paramf('LPE_FLW_R', 4),
|
||||
get_paramf('LPE_FLW_RR', 4))
|
||||
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'))
|
||||
elif est == 2:
|
||||
fuse = get_param('EKF2_AID_MASK', 0)
|
||||
fuse = get_param('EKF2_AID_MASK')
|
||||
if not fuse & (1 << 1):
|
||||
failure('optical flow fusion is disabled, change EKF2_AID_MASK parameter')
|
||||
delay = get_param('EKF2_OF_DELAY', 0)
|
||||
delay = get_param('EKF2_OF_DELAY')
|
||||
if delay != 0:
|
||||
failure('EKF2_OF_DELAY = %.2f, but it should be zero', delay)
|
||||
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))
|
||||
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'))
|
||||
|
||||
except rospy.ROSException:
|
||||
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')
|
||||
failure('no optical flow data (from Raspberry)')
|
||||
|
||||
|
||||
@check('Rangefinder')
|
||||
@@ -684,7 +588,7 @@ def check_rangefinder():
|
||||
|
||||
est = get_param('SYS_MC_EST_GROUP')
|
||||
if est == 1:
|
||||
fuse = get_param('LPE_FUSION', 0)
|
||||
fuse = get_param('LPE_FUSION')
|
||||
if not fuse & (1 << 5):
|
||||
info('"pub agl as lpos down" in LPE_FUSION is disabled, NOT operating over flat surface')
|
||||
else:
|
||||
@@ -705,20 +609,16 @@ 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 > 20:
|
||||
if duration > 15:
|
||||
failure('long Raspbian boot duration: %ss (systemd-analyze for analyzing)', duration)
|
||||
|
||||
|
||||
@check('CPU usage')
|
||||
def check_cpu_usage():
|
||||
WHITELIST = 'nodelet', 'gzclient', 'gzserver', 'selfcheck.py'
|
||||
WHITELIST = 'nodelet',
|
||||
CMD = "top -n 1 -b -i | tail -n +8 | awk '{ printf(\"%-8s\\t%-8s\\t%-8s\\n\", $1, $9, $12); }'"
|
||||
output = subprocess.check_output(CMD, shell=True).decode()
|
||||
processes = output.split('\n')
|
||||
@@ -727,16 +627,13 @@ def check_cpu_usage():
|
||||
continue
|
||||
pid, cpu, cmd = process.split('\t')
|
||||
|
||||
if cmd.strip() not in WHITELIST and locale.atof(cpu) > 30:
|
||||
if cmd.strip() not in WHITELIST and float(cpu) > 30:
|
||||
failure('high CPU usage (%s%%) detected: %s (PID %s)',
|
||||
cpu.strip(), cmd.strip(), pid.strip())
|
||||
|
||||
|
||||
@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()
|
||||
@@ -749,22 +646,13 @@ def check_clover_service():
|
||||
elif 'failed' in output:
|
||||
failure('service failed to run, check your launch-files')
|
||||
|
||||
BLACKLIST = 'Unexpected command 520', 'Time jump detected', 'different index:'
|
||||
|
||||
r = re.compile(r'^(.*)\[(FATAL|ERROR| WARN)\] \[\d+.\d+\]: (.*?)(\x1b(.*))?$')
|
||||
r = re.compile(r'^(.*)\[(FATAL|ERROR)\] \[\d+.\d+\]: (.*?)(\x1b(.*))?$')
|
||||
error_count = OrderedDict()
|
||||
try:
|
||||
for line in open('/tmp/clover.err', 'r'):
|
||||
skip = False
|
||||
for substr in BLACKLIST:
|
||||
if substr in line:
|
||||
skip = True
|
||||
if skip:
|
||||
continue
|
||||
|
||||
node_error = r.search(line)
|
||||
if node_error:
|
||||
msg = node_error.groups()[1].strip() + ': ' + node_error.groups()[2]
|
||||
msg = node_error.groups()[1] + ': ' + node_error.groups()[2]
|
||||
if msg in error_count:
|
||||
error_count[msg] += 1
|
||||
else:
|
||||
@@ -787,18 +675,11 @@ def check_image():
|
||||
try:
|
||||
info('version: %s', open('/etc/clover_version').read().strip())
|
||||
except IOError:
|
||||
try:
|
||||
info('VM version: %s', open('/etc/clover_vm_version').read().strip())
|
||||
except IOError:
|
||||
info('no /etc/clover_version file, not the Clover image?')
|
||||
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')
|
||||
@@ -820,10 +701,6 @@ 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:
|
||||
@@ -846,14 +723,6 @@ 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?
|
||||
@@ -884,7 +753,7 @@ def check_rpi_health():
|
||||
# with some of the FLAGs OR'ed together
|
||||
output = subprocess.check_output(['vcgencmd', 'get_throttled']).decode()
|
||||
except OSError:
|
||||
info('could not call vcgencmd binary; not a Raspberry Pi?')
|
||||
failure('could not call vcgencmd binary; not a Raspberry Pi?')
|
||||
return
|
||||
|
||||
throttle_mask = int(output.split('=')[1], base=16)
|
||||
@@ -901,47 +770,26 @@ def check_board():
|
||||
info('could not open /proc/device-tree/model, not a Raspberry Pi?')
|
||||
|
||||
|
||||
def parallel_for(fns):
|
||||
threads = []
|
||||
for fn in fns:
|
||||
thread = Thread(target=fn)
|
||||
thread.start()
|
||||
threads.append(thread)
|
||||
for thread in threads:
|
||||
thread.join()
|
||||
|
||||
|
||||
def consequentially_for(fns):
|
||||
for fn in fns:
|
||||
fn()
|
||||
|
||||
|
||||
def selfcheck():
|
||||
checks = [
|
||||
check_image,
|
||||
check_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)
|
||||
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 __name__ == '__main__':
|
||||
|
||||
@@ -23,14 +23,12 @@
|
||||
#include <tf2_ros/static_transform_broadcaster.h>
|
||||
#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
|
||||
#include <std_srvs/Trigger.h>
|
||||
#include <geometry_msgs/PointStamped.h>
|
||||
#include <geometry_msgs/PoseStamped.h>
|
||||
#include <geometry_msgs/TwistStamped.h>
|
||||
#include <geometry_msgs/Vector3Stamped.h>
|
||||
#include <geometry_msgs/QuaternionStamped.h>
|
||||
#include <sensor_msgs/NavSatFix.h>
|
||||
#include <sensor_msgs/BatteryState.h>
|
||||
#include <sensor_msgs/Range.h>
|
||||
#include <mavros_msgs/CommandBool.h>
|
||||
#include <mavros_msgs/SetMode.h>
|
||||
#include <mavros_msgs/PositionTarget.h>
|
||||
@@ -39,19 +37,14 @@
|
||||
#include <mavros_msgs/State.h>
|
||||
#include <mavros_msgs/StatusText.h>
|
||||
#include <mavros_msgs/ManualControl.h>
|
||||
#include <mavros_msgs/Altitude.h>
|
||||
|
||||
#include <clover/GetTelemetry.h>
|
||||
#include <clover/Navigate.h>
|
||||
#include <clover/NavigateGlobal.h>
|
||||
#include <clover/SetAltitude.h>
|
||||
#include <clover/SetYaw.h>
|
||||
#include <clover/SetYawRate.h>
|
||||
#include <clover/SetPosition.h>
|
||||
#include <clover/SetVelocity.h>
|
||||
#include <clover/SetAttitude.h>
|
||||
#include <clover/SetRates.h>
|
||||
#include <clover/State.h>
|
||||
|
||||
using std::string;
|
||||
using std::isnan;
|
||||
@@ -61,7 +54,6 @@ using namespace clover;
|
||||
using mavros_msgs::PositionTarget;
|
||||
using mavros_msgs::AttitudeTarget;
|
||||
using mavros_msgs::Thrust;
|
||||
using mavros_msgs::Altitude;
|
||||
|
||||
// tf2
|
||||
tf2_ros::Buffer tf_buffer;
|
||||
@@ -69,7 +61,6 @@ 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;
|
||||
@@ -87,43 +78,35 @@ float default_speed;
|
||||
bool auto_release;
|
||||
bool land_only_in_offboard, nav_from_sp, check_kill_switch;
|
||||
std::map<string, string> reference_frames;
|
||||
string terrain_frame_mode;
|
||||
|
||||
// Publishers
|
||||
ros::Publisher attitude_pub, attitude_raw_pub, position_pub, position_raw_pub, rates_pub, thrust_pub, state_pub;
|
||||
ros::Publisher attitude_pub, attitude_raw_pub, position_pub, position_raw_pub, rates_pub, thrust_pub;
|
||||
|
||||
// Service clients
|
||||
ros::ServiceClient arming, set_mode;
|
||||
|
||||
// Containers
|
||||
ros::Timer setpoint_timer;
|
||||
tf::Quaternion tq;
|
||||
PoseStamped position_msg;
|
||||
PositionTarget position_raw_msg;
|
||||
//TwistStamped rates_msg;
|
||||
AttitudeTarget att_raw_msg;
|
||||
Thrust thrust_msg;
|
||||
TwistStamped rates_msg;
|
||||
TransformStamped target, setpoint;
|
||||
geometry_msgs::TransformStamped body;
|
||||
geometry_msgs::TransformStamped terrain;
|
||||
|
||||
// State
|
||||
PoseStamped nav_start;
|
||||
PointStamped setpoint_position;
|
||||
PointStamped setpoint_altitude;
|
||||
Vector3Stamped setpoint_velocity;
|
||||
float setpoint_yaw, setpoint_roll, setpoint_pitch;
|
||||
Vector3 setpoint_rates;
|
||||
string yaw_frame_id;
|
||||
float setpoint_thrust;
|
||||
PoseStamped setpoint_position, setpoint_position_transformed;
|
||||
Vector3Stamped setpoint_velocity, setpoint_velocity_transformed;
|
||||
QuaternionStamped setpoint_attitude, setpoint_attitude_transformed;
|
||||
float setpoint_yaw_rate;
|
||||
float nav_speed;
|
||||
float setpoint_lat = NAN, setpoint_lon = NAN;
|
||||
bool busy = false;
|
||||
bool wait_armed = false;
|
||||
bool nav_from_sp_flag = false;
|
||||
|
||||
// Last published
|
||||
PoseStamped setpoint_pose_local;
|
||||
Vector3Stamped setpoint_velocity_local;
|
||||
float yaw_local;
|
||||
|
||||
enum setpoint_type_t {
|
||||
NONE,
|
||||
NAVIGATE,
|
||||
@@ -131,10 +114,7 @@ enum setpoint_type_t {
|
||||
POSITION,
|
||||
VELOCITY,
|
||||
ATTITUDE,
|
||||
RATES,
|
||||
_ALTITUDE,
|
||||
_YAW,
|
||||
_YAW_RATE,
|
||||
RATES
|
||||
};
|
||||
|
||||
enum setpoint_type_t setpoint_type = NONE;
|
||||
@@ -169,9 +149,6 @@ 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));
|
||||
@@ -189,7 +166,7 @@ void handleLocalPosition(const PoseStamped& pose)
|
||||
{
|
||||
local_position = pose;
|
||||
publishBodyFrame();
|
||||
// TODO: home?
|
||||
// TODO: terrain?, home?
|
||||
}
|
||||
|
||||
// wait for transform without interrupting publishing setpoints
|
||||
@@ -204,30 +181,6 @@ inline bool waitTransform(const string& target, const string& source,
|
||||
ros::spinOnce();
|
||||
r.sleep();
|
||||
}
|
||||
return false;
|
||||
}
|
||||
|
||||
void publishTerrain(const double distance, const ros::Time& stamp)
|
||||
{
|
||||
if (!waitTransform(local_frame, body.child_frame_id, stamp, ros::Duration(0.1))) return;
|
||||
|
||||
auto t = tf_buffer.lookupTransform(local_frame, body.child_frame_id, stamp);
|
||||
t.child_frame_id = terrain.child_frame_id;
|
||||
t.transform.translation.z -= distance;
|
||||
static_transform_broadcaster->sendTransform(t);
|
||||
}
|
||||
|
||||
void handleAltitude(const Altitude& alt)
|
||||
{
|
||||
if (!std::isfinite(alt.bottom_clearance)) return;
|
||||
publishTerrain(alt.bottom_clearance, alt.header.stamp);
|
||||
}
|
||||
|
||||
void handleRange(const Range& range)
|
||||
{
|
||||
if (!std::isfinite(range.range)) return;
|
||||
// TODO: check it's facing down
|
||||
publishTerrain(range.range, range.header.stamp);
|
||||
}
|
||||
|
||||
#define TIMEOUT(msg, timeout) (msg.header.stamp.isZero() || (ros::Time::now() - msg.header.stamp > timeout))
|
||||
@@ -249,11 +202,11 @@ bool getTelemetry(GetTelemetry::Request& req, GetTelemetry::Response& res)
|
||||
res.vx = NAN;
|
||||
res.vy = NAN;
|
||||
res.vz = NAN;
|
||||
res.roll = NAN;
|
||||
res.pitch = NAN;
|
||||
res.roll = NAN;
|
||||
res.yaw = NAN;
|
||||
res.roll_rate = NAN;
|
||||
res.pitch_rate = NAN;
|
||||
res.roll_rate = NAN;
|
||||
res.yaw_rate = NAN;
|
||||
res.voltage = NAN;
|
||||
res.cell_voltage = NAN;
|
||||
@@ -383,20 +336,20 @@ inline float getDistance(const Point& from, const Point& to)
|
||||
return hypot(from.x - to.x, from.y - to.y, from.z - to.z);
|
||||
}
|
||||
|
||||
void getNavigateSetpoint(const ros::Time& stamp, const float speed, Point& nav_setpoint)
|
||||
void getNavigateSetpoint(const ros::Time& stamp, float speed, Point& nav_setpoint)
|
||||
{
|
||||
if (wait_armed) {
|
||||
// don't start navigating if we're waiting arming
|
||||
nav_start.header.stamp = stamp;
|
||||
}
|
||||
|
||||
float distance = getDistance(nav_start.pose.position, setpoint_pose_local.pose.position);
|
||||
float distance = getDistance(nav_start.pose.position, setpoint_position_transformed.pose.position);
|
||||
float time = distance / speed;
|
||||
float passed = std::min((stamp - nav_start.header.stamp).toSec() / time, 1.0);
|
||||
|
||||
nav_setpoint.x = nav_start.pose.position.x + (setpoint_pose_local.pose.position.x - nav_start.pose.position.x) * passed;
|
||||
nav_setpoint.y = nav_start.pose.position.y + (setpoint_pose_local.pose.position.y - nav_start.pose.position.y) * passed;
|
||||
nav_setpoint.z = nav_start.pose.position.z + (setpoint_pose_local.pose.position.z - nav_start.pose.position.z) * passed;
|
||||
nav_setpoint.x = nav_start.pose.position.x + (setpoint_position_transformed.pose.position.x - nav_start.pose.position.x) * passed;
|
||||
nav_setpoint.y = nav_start.pose.position.y + (setpoint_position_transformed.pose.position.y - nav_start.pose.position.y) * passed;
|
||||
nav_setpoint.z = nav_start.pose.position.z + (setpoint_position_transformed.pose.position.z - nav_start.pose.position.z) * passed;
|
||||
}
|
||||
|
||||
PoseStamped globalToLocal(double lat, double lon)
|
||||
@@ -427,101 +380,44 @@ PoseStamped globalToLocal(double lat, double lon)
|
||||
return pose;
|
||||
}
|
||||
|
||||
// publish navigate_target frame
|
||||
void publishTarget(ros::Time stamp, bool _static = false)
|
||||
{
|
||||
bool single_frame = (setpoint_position.header.frame_id == setpoint_altitude.header.frame_id);
|
||||
|
||||
// handle yaw for target frame
|
||||
if (setpoint_yaw_type == YAW || setpoint_yaw_type == YAW_RATE) { // use last set yaw for yaw_rate
|
||||
if (setpoint_altitude.header.frame_id == yaw_frame_id) {
|
||||
target.transform.rotation = tf::createQuaternionMsgFromYaw(setpoint_yaw);
|
||||
} else {
|
||||
single_frame = false;
|
||||
target.transform.rotation = tf::createQuaternionMsgFromYaw(yaw_local);
|
||||
}
|
||||
} else if (setpoint_yaw_type == TOWARDS) {
|
||||
single_frame = false;
|
||||
target.transform.rotation = tf::createQuaternionMsgFromYaw(yaw_local);
|
||||
}
|
||||
|
||||
if (_static && single_frame) {
|
||||
// publish at user's command, if all frames are the same
|
||||
target.header.frame_id = setpoint_position.header.frame_id;
|
||||
target.header.stamp = stamp;
|
||||
target.transform.translation.x = setpoint_position.point.x;
|
||||
target.transform.translation.y = setpoint_position.point.y;
|
||||
target.transform.translation.z = setpoint_position.point.z;
|
||||
|
||||
} else if (!_static) {
|
||||
// publish at each iteration, if frames are different
|
||||
target.header = setpoint_pose_local.header;
|
||||
target.transform.translation.x = setpoint_pose_local.pose.position.x;
|
||||
target.transform.translation.y = setpoint_pose_local.pose.position.y;
|
||||
target.transform.translation.z = setpoint_pose_local.pose.position.z;
|
||||
}
|
||||
|
||||
static_transform_broadcaster->sendTransform(target);
|
||||
}
|
||||
|
||||
void publish(const ros::Time stamp)
|
||||
{
|
||||
if (setpoint_type == NONE) return;
|
||||
|
||||
position_raw_msg.header.stamp = stamp;
|
||||
thrust_msg.header.stamp = stamp;
|
||||
rates_msg.header.stamp = stamp;
|
||||
|
||||
// transform position
|
||||
if (setpoint_type == NAVIGATE || setpoint_type == NAVIGATE_GLOBAL || setpoint_type == POSITION) {
|
||||
setpoint_position.header.stamp = stamp;
|
||||
setpoint_altitude.header.stamp = stamp;
|
||||
// transform xy
|
||||
try {
|
||||
auto xy = tf_buffer.transform(setpoint_position, local_frame, ros::Duration(0.05));
|
||||
setpoint_pose_local.header = xy.header;
|
||||
setpoint_pose_local.pose.position.x = xy.point.x;
|
||||
setpoint_pose_local.pose.position.y = xy.point.y;
|
||||
} catch (tf2::TransformException& ex) {
|
||||
// can't transform xy, use last known
|
||||
ROS_WARN_THROTTLE(10, "can't transform: %s", ex.what());
|
||||
try {
|
||||
// transform position and/or yaw
|
||||
if (setpoint_type == NAVIGATE || setpoint_type == NAVIGATE_GLOBAL || setpoint_type == POSITION || setpoint_type == VELOCITY || setpoint_type == ATTITUDE) {
|
||||
setpoint_position.header.stamp = stamp;
|
||||
tf_buffer.transform(setpoint_position, setpoint_position_transformed, local_frame, ros::Duration(0.05));
|
||||
}
|
||||
// transform altitude
|
||||
try {
|
||||
setpoint_pose_local.pose.position.z = tf_buffer.transform(setpoint_altitude, local_frame, ros::Duration(0.05)).point.z;
|
||||
} catch (tf2::TransformException& ex) {
|
||||
// can't transform altitude, use last known
|
||||
ROS_WARN_THROTTLE(10, "can't transform: %s", ex.what());
|
||||
|
||||
// transform velocity
|
||||
if (setpoint_type == VELOCITY) {
|
||||
setpoint_velocity.header.stamp = stamp;
|
||||
tf_buffer.transform(setpoint_velocity, setpoint_velocity_transformed, local_frame, ros::Duration(0.05));
|
||||
}
|
||||
|
||||
} catch (const tf2::TransformException& e) {
|
||||
ROS_WARN_THROTTLE(10, "can't transform");
|
||||
}
|
||||
|
||||
// transform yaw
|
||||
if (setpoint_yaw_type == YAW) {
|
||||
try {
|
||||
QuaternionStamped q;
|
||||
q.header.stamp = stamp;
|
||||
q.header.frame_id = yaw_frame_id;
|
||||
q.quaternion = tf::createQuaternionMsgFromYaw(setpoint_yaw);
|
||||
yaw_local = tf2::getYaw(tf_buffer.transform(q, local_frame, ros::Duration(0.05)).quaternion);
|
||||
} catch (tf2::TransformException& ex) {
|
||||
// can't transform yaw, use last known
|
||||
ROS_WARN_THROTTLE(10, "can't transform: %s", ex.what());
|
||||
}
|
||||
}
|
||||
|
||||
// compute navigate setpoint
|
||||
if (setpoint_type == NAVIGATE || setpoint_type == NAVIGATE_GLOBAL) {
|
||||
position_msg.pose.orientation = setpoint_position_transformed.pose.orientation; // copy yaw
|
||||
getNavigateSetpoint(stamp, nav_speed, position_msg.pose.position);
|
||||
|
||||
if (setpoint_yaw_type == TOWARDS) {
|
||||
yaw_local = atan2(position_msg.pose.position.y - nav_start.pose.position.y,
|
||||
position_msg.pose.position.x - nav_start.pose.position.x);
|
||||
double yaw_towards = atan2(position_msg.pose.position.y - nav_start.pose.position.y,
|
||||
position_msg.pose.position.x - nav_start.pose.position.x);
|
||||
position_msg.pose.orientation = tf::createQuaternionMsgFromRollPitchYaw(0, 0, yaw_towards);
|
||||
}
|
||||
|
||||
position_msg.pose.orientation = tf::createQuaternionMsgFromYaw(yaw_local);
|
||||
}
|
||||
|
||||
if (setpoint_type == POSITION) {
|
||||
position_msg = setpoint_pose_local;
|
||||
position_msg.pose.orientation = tf::createQuaternionMsgFromYaw(yaw_local);
|
||||
position_msg = setpoint_position_transformed;
|
||||
}
|
||||
|
||||
if (setpoint_type == POSITION || setpoint_type == NAVIGATE || setpoint_type == NAVIGATE_GLOBAL) {
|
||||
@@ -538,14 +434,14 @@ void publish(const ros::Time stamp)
|
||||
PositionTarget::IGNORE_AFY +
|
||||
PositionTarget::IGNORE_AFZ +
|
||||
PositionTarget::IGNORE_YAW;
|
||||
position_raw_msg.yaw_rate = setpoint_rates.z;
|
||||
position_raw_msg.yaw_rate = setpoint_yaw_rate;
|
||||
position_raw_msg.position = position_msg.pose.position;
|
||||
position_raw_pub.publish(position_raw_msg);
|
||||
}
|
||||
|
||||
// publish setpoint frame
|
||||
if (!setpoint.child_frame_id.empty()) {
|
||||
if (setpoint.header.stamp >= position_msg.header.stamp) {
|
||||
if (setpoint.header.stamp == position_msg.header.stamp) {
|
||||
return; // avoid TF_REPEATED_DATA warnings
|
||||
}
|
||||
|
||||
@@ -557,22 +453,9 @@ void publish(const ros::Time stamp)
|
||||
setpoint.header.stamp = position_msg.header.stamp;
|
||||
transform_broadcaster->sendTransform(setpoint);
|
||||
}
|
||||
|
||||
// publish dynamic target frame
|
||||
publishTarget(stamp);
|
||||
}
|
||||
|
||||
if (setpoint_type == VELOCITY) {
|
||||
// transform velocity to local frame
|
||||
setpoint_velocity.header.stamp = stamp;
|
||||
try {
|
||||
setpoint_velocity_local = tf_buffer.transform(setpoint_velocity, local_frame, ros::Duration(0.05));
|
||||
} catch (tf2::TransformException& ex) {
|
||||
// can't transform velocity, use last known
|
||||
ROS_WARN_THROTTLE(10, "can't transform: %s", ex.what());
|
||||
}
|
||||
|
||||
// publish velocity
|
||||
position_raw_msg.type_mask = PositionTarget::IGNORE_PX +
|
||||
PositionTarget::IGNORE_PY +
|
||||
PositionTarget::IGNORE_PZ +
|
||||
@@ -580,22 +463,14 @@ void publish(const ros::Time stamp)
|
||||
PositionTarget::IGNORE_AFY +
|
||||
PositionTarget::IGNORE_AFZ;
|
||||
position_raw_msg.type_mask += setpoint_yaw_type == YAW ? PositionTarget::IGNORE_YAW_RATE : PositionTarget::IGNORE_YAW;
|
||||
position_raw_msg.velocity = setpoint_velocity_local.vector;
|
||||
position_raw_msg.yaw = yaw_local;
|
||||
position_raw_msg.yaw_rate = setpoint_rates.z;
|
||||
position_raw_msg.velocity = setpoint_velocity_transformed.vector;
|
||||
position_raw_msg.yaw = tf2::getYaw(setpoint_position_transformed.pose.orientation);
|
||||
position_raw_msg.yaw_rate = setpoint_yaw_rate;
|
||||
position_raw_pub.publish(position_raw_msg);
|
||||
}
|
||||
|
||||
if (setpoint_type == ATTITUDE) {
|
||||
PoseStamped msg;
|
||||
msg.header.stamp = stamp;
|
||||
msg.header.frame_id = local_frame;
|
||||
msg.pose.orientation = tf::createQuaternionMsgFromRollPitchYaw(setpoint_roll, setpoint_pitch, yaw_local);
|
||||
attitude_pub.publish(msg);
|
||||
|
||||
Thrust thrust_msg;
|
||||
thrust_msg.header.stamp = stamp;
|
||||
thrust_msg.thrust = setpoint_thrust;
|
||||
attitude_pub.publish(setpoint_position_transformed);
|
||||
thrust_pub.publish(thrust_msg);
|
||||
}
|
||||
|
||||
@@ -604,12 +479,11 @@ void publish(const ros::Time stamp)
|
||||
// thrust_pub.publish(thrust_msg);
|
||||
// mavros rates topics waits for rates in local frame
|
||||
// use rates in body frame for simplicity
|
||||
AttitudeTarget att_raw_msg;
|
||||
att_raw_msg.header.stamp = stamp;
|
||||
att_raw_msg.header.frame_id = fcu_frame;
|
||||
att_raw_msg.type_mask = AttitudeTarget::IGNORE_ATTITUDE;
|
||||
att_raw_msg.body_rate = setpoint_rates;
|
||||
att_raw_msg.thrust = setpoint_thrust;
|
||||
att_raw_msg.body_rate = rates_msg.twist.angular;
|
||||
att_raw_msg.thrust = thrust_msg.thrust;
|
||||
attitude_raw_pub.publish(att_raw_msg);
|
||||
}
|
||||
}
|
||||
@@ -649,59 +523,10 @@ inline void checkState()
|
||||
throw std::runtime_error("No connection to FCU, https://clover.coex.tech/connection");
|
||||
}
|
||||
|
||||
void publishState()
|
||||
{
|
||||
clover::State msg;
|
||||
msg.mode = setpoint_type;
|
||||
msg.yaw_mode = setpoint_yaw_type;
|
||||
|
||||
if (setpoint_position.header.frame_id.empty()) {
|
||||
msg.x = NAN;
|
||||
msg.y = NAN;
|
||||
msg.z = NAN;
|
||||
} else {
|
||||
msg.x = setpoint_position.point.x;
|
||||
msg.y = setpoint_position.point.y;
|
||||
msg.z = setpoint_altitude.point.z;
|
||||
}
|
||||
|
||||
msg.speed = nav_speed;
|
||||
msg.lat = setpoint_lat;
|
||||
msg.lon = setpoint_lon;
|
||||
msg.vx = setpoint_velocity.vector.x;
|
||||
msg.vy = setpoint_velocity.vector.y;
|
||||
msg.vz = setpoint_velocity.vector.z;
|
||||
msg.roll = setpoint_roll;
|
||||
msg.pitch = setpoint_pitch;
|
||||
msg.yaw = !yaw_frame_id.empty() ? setpoint_yaw : NAN;
|
||||
|
||||
msg.roll_rate = setpoint_rates.x;
|
||||
msg.pitch_rate = setpoint_rates.y;
|
||||
msg.yaw_rate = setpoint_rates.z;
|
||||
msg.thrust = setpoint_thrust;
|
||||
|
||||
if (setpoint_type == VELOCITY) {
|
||||
msg.xy_frame_id = setpoint_velocity.header.frame_id;
|
||||
msg.z_frame_id = setpoint_velocity.header.frame_id;
|
||||
} else {
|
||||
msg.xy_frame_id = setpoint_position.header.frame_id;
|
||||
msg.z_frame_id = setpoint_altitude.header.frame_id;
|
||||
}
|
||||
msg.yaw_frame_id = yaw_frame_id;
|
||||
|
||||
state_pub.publish(msg);
|
||||
}
|
||||
|
||||
inline float safe(float value) {
|
||||
return std::isfinite(value) ? value : 0;
|
||||
}
|
||||
|
||||
#define ENSURE_FINITE(var) { if (!std::isfinite(var)) throw std::runtime_error(#var " argument cannot be NaN or Inf"); }
|
||||
|
||||
#define ENSURE_NON_INF(var) { if (std::isinf(var)) throw std::runtime_error(#var " argument cannot be Inf"); }
|
||||
|
||||
bool serve(enum setpoint_type_t sp_type, float x, float y, float z, float vx, float vy, float vz,
|
||||
float roll, float pitch, float yaw, float roll_rate, float pitch_rate, float yaw_rate, // editorconfig-checker-disable-line
|
||||
float pitch, float roll, float yaw, float pitch_rate, float roll_rate, float yaw_rate, // editorconfig-checker-disable-line
|
||||
float lat, float lon, float thrust, float speed, string frame_id, bool auto_arm, // editorconfig-checker-disable-line
|
||||
uint8_t& success, string& message) // editorconfig-checker-disable-line
|
||||
{
|
||||
@@ -728,40 +553,69 @@ bool serve(enum setpoint_type_t sp_type, float x, float y, float z, float vx, fl
|
||||
auto search = reference_frames.find(frame_id);
|
||||
const string& reference_frame = search == reference_frames.end() ? frame_id : search->second;
|
||||
|
||||
ENSURE_NON_INF(x);
|
||||
ENSURE_NON_INF(y);
|
||||
ENSURE_NON_INF(z);
|
||||
ENSURE_NON_INF(speed); // TODO: allow inf
|
||||
ENSURE_NON_INF(vx);
|
||||
ENSURE_NON_INF(vy);
|
||||
ENSURE_NON_INF(vz);
|
||||
ENSURE_NON_INF(roll);
|
||||
ENSURE_NON_INF(pitch);
|
||||
ENSURE_NON_INF(roll_rate);
|
||||
ENSURE_NON_INF(pitch_rate);
|
||||
ENSURE_NON_INF(yaw_rate);
|
||||
ENSURE_NON_INF(thrust);
|
||||
// Serve "partial" commands
|
||||
|
||||
if (sp_type == NAVIGATE_GLOBAL) {
|
||||
if (!auto_arm && std::isfinite(yaw) &&
|
||||
isnan(x) && isnan(y) && isnan(z) && isnan(vx) && isnan(vy) && isnan(vz) &&
|
||||
isnan(pitch) && isnan(roll) && isnan(thrust) &&
|
||||
isnan(lat) && isnan(lon)) {
|
||||
// change only the yaw
|
||||
if (setpoint_type == POSITION || setpoint_type == NAVIGATE || setpoint_type == NAVIGATE_GLOBAL || setpoint_type == VELOCITY) {
|
||||
if (!waitTransform(setpoint_position.header.frame_id, frame_id, stamp, transform_timeout))
|
||||
throw std::runtime_error("Can't transform from " + frame_id + " to " + setpoint_position.header.frame_id);
|
||||
|
||||
message = "Changing yaw only";
|
||||
|
||||
QuaternionStamped q;
|
||||
q.header.frame_id = frame_id;
|
||||
q.header.stamp = stamp;
|
||||
q.quaternion = tf::createQuaternionMsgFromYaw(yaw); // TODO: pitch=0, roll=0 is not totally correct
|
||||
setpoint_position.pose.orientation = tf_buffer.transform(q, setpoint_position.header.frame_id).quaternion;
|
||||
setpoint_yaw_type = YAW;
|
||||
goto publish_setpoint;
|
||||
} else {
|
||||
throw std::runtime_error("Setting yaw is possible only when position or velocity setpoints active");
|
||||
}
|
||||
}
|
||||
|
||||
if (!auto_arm && std::isfinite(yaw_rate) &&
|
||||
isnan(x) && isnan(y) && isnan(z) && isnan(vx) && isnan(vy) && isnan(vz) &&
|
||||
isnan(pitch) && isnan(roll) && isnan(yaw) && isnan(thrust) &&
|
||||
isnan(lat) && isnan(lon)) {
|
||||
// change only the yaw rate
|
||||
if (setpoint_type == POSITION || setpoint_type == NAVIGATE || setpoint_type == NAVIGATE_GLOBAL || setpoint_type == VELOCITY) {
|
||||
message = "Changing yaw rate only";
|
||||
|
||||
setpoint_yaw_type = YAW_RATE;
|
||||
setpoint_yaw_rate = yaw_rate;
|
||||
goto publish_setpoint;
|
||||
} else {
|
||||
throw std::runtime_error("Setting yaw rate is possible only when position or velocity setpoints active");
|
||||
}
|
||||
}
|
||||
|
||||
// Serve normal commands
|
||||
|
||||
if (sp_type == NAVIGATE || sp_type == POSITION) {
|
||||
ENSURE_FINITE(x);
|
||||
ENSURE_FINITE(y);
|
||||
ENSURE_FINITE(z);
|
||||
} else if (sp_type == NAVIGATE_GLOBAL) {
|
||||
ENSURE_FINITE(lat);
|
||||
ENSURE_FINITE(lon);
|
||||
}
|
||||
|
||||
if (isfinite(x) != isfinite(y)) {
|
||||
throw std::runtime_error("x and y can be set only together");
|
||||
}
|
||||
|
||||
if (isfinite(yaw_rate)) {
|
||||
if (sp_type > RATES && setpoint_type == ATTITUDE) {
|
||||
throw std::runtime_error("Yaw rate cannot be set in attitude mode.");
|
||||
}
|
||||
}
|
||||
|
||||
// set_altitude
|
||||
if (sp_type == _ALTITUDE) {
|
||||
if (setpoint_type == VELOCITY || setpoint_type == ATTITUDE || setpoint_type == RATES) {
|
||||
throw std::runtime_error("Altitude cannot be set in velocity, attitude or rates mode.");
|
||||
}
|
||||
ENSURE_FINITE(z);
|
||||
} else if (sp_type == VELOCITY) {
|
||||
ENSURE_FINITE(vx);
|
||||
ENSURE_FINITE(vy);
|
||||
ENSURE_FINITE(vz);
|
||||
} else if (sp_type == ATTITUDE) {
|
||||
ENSURE_FINITE(pitch);
|
||||
ENSURE_FINITE(roll);
|
||||
ENSURE_FINITE(thrust);
|
||||
} else if (sp_type == RATES) {
|
||||
ENSURE_FINITE(pitch_rate);
|
||||
ENSURE_FINITE(roll_rate);
|
||||
ENSURE_FINITE(thrust);
|
||||
}
|
||||
|
||||
if (sp_type == NAVIGATE || sp_type == NAVIGATE_GLOBAL) {
|
||||
@@ -775,13 +629,20 @@ bool serve(enum setpoint_type_t sp_type, float x, float y, float z, float vx, fl
|
||||
speed = default_speed;
|
||||
}
|
||||
|
||||
if (sp_type == NAVIGATE || sp_type == NAVIGATE_GLOBAL || sp_type == POSITION || sp_type == VELOCITY) {
|
||||
if (yaw_rate != 0 && !std::isnan(yaw))
|
||||
throw std::runtime_error("Yaw value should be NaN for setting yaw rate");
|
||||
|
||||
if (std::isnan(yaw_rate) && std::isnan(yaw))
|
||||
throw std::runtime_error("Both yaw and yaw_rate cannot be NaN");
|
||||
}
|
||||
|
||||
if (sp_type == NAVIGATE_GLOBAL) {
|
||||
if (TIMEOUT(global_position, global_position_timeout))
|
||||
throw std::runtime_error("No global position");
|
||||
}
|
||||
|
||||
// if any value need to be transformed to reference frame
|
||||
if (isfinite(x) || isfinite(y) || isfinite(z) || isfinite(vx) || isfinite(vy) || isfinite(vz) || isfinite(yaw)) {
|
||||
if (sp_type == NAVIGATE || sp_type == NAVIGATE_GLOBAL || sp_type == POSITION || sp_type == VELOCITY || sp_type == ATTITUDE) {
|
||||
// make sure transform from frame_id to reference frame available
|
||||
if (!waitTransform(reference_frame, frame_id, stamp, transform_timeout))
|
||||
throw std::runtime_error("Can't transform from " + frame_id + " to " + reference_frame);
|
||||
@@ -798,27 +659,15 @@ bool serve(enum setpoint_type_t sp_type, float x, float y, float z, float vx, fl
|
||||
auto xy_in_req_frame = tf_buffer.transform(pose_local, frame_id);
|
||||
x = xy_in_req_frame.pose.position.x;
|
||||
y = xy_in_req_frame.pose.position.y;
|
||||
setpoint_lat = lat;
|
||||
setpoint_lon = lon;
|
||||
}
|
||||
|
||||
// Everything fine - switch setpoint type
|
||||
if (sp_type <= RATES) {
|
||||
setpoint_type = sp_type;
|
||||
}
|
||||
setpoint_type = sp_type;
|
||||
|
||||
if (setpoint_type != NAVIGATE && setpoint_type != NAVIGATE_GLOBAL) {
|
||||
if (sp_type != NAVIGATE && sp_type != NAVIGATE_GLOBAL) {
|
||||
nav_from_sp_flag = false;
|
||||
}
|
||||
|
||||
bool to_auto_arm = auto_arm && (state.mode != "OFFBOARD" || !state.armed);
|
||||
if (to_auto_arm || setpoint_type == VELOCITY || setpoint_type == ATTITUDE || setpoint_type == RATES) {
|
||||
// invalidate position setpoint
|
||||
setpoint_position.header.frame_id = "";
|
||||
setpoint_altitude.header.frame_id = "";
|
||||
yaw_frame_id = "";
|
||||
}
|
||||
|
||||
if (sp_type == NAVIGATE || sp_type == NAVIGATE_GLOBAL) {
|
||||
// starting point
|
||||
if (nav_from_sp && nav_from_sp_flag) {
|
||||
@@ -827,139 +676,84 @@ bool serve(enum setpoint_type_t sp_type, float x, float y, float z, float vx, fl
|
||||
} else {
|
||||
nav_start = local_position;
|
||||
}
|
||||
|
||||
if (!isnan(speed)) {
|
||||
nav_speed = speed;
|
||||
}
|
||||
|
||||
nav_speed = speed;
|
||||
nav_from_sp_flag = true;
|
||||
}
|
||||
|
||||
// handle position
|
||||
if (setpoint_type == NAVIGATE || setpoint_type == NAVIGATE_GLOBAL || setpoint_type == POSITION) {
|
||||
// if (sp_type == NAVIGATE || sp_type == NAVIGATE_GLOBAL || sp_type == POSITION || sp_type == VELOCITY) {
|
||||
// if (std::isnan(yaw) && yaw_rate == 0) {
|
||||
// // keep yaw unchanged
|
||||
// // TODO: this is incorrect, because we need yaw in desired frame
|
||||
// yaw = tf2::getYaw(local_position.pose.orientation);
|
||||
// }
|
||||
// }
|
||||
|
||||
PointStamped desired;
|
||||
desired.header.frame_id = frame_id;
|
||||
desired.header.stamp = stamp;
|
||||
desired.point.x = safe(x);
|
||||
desired.point.y = safe(y);
|
||||
desired.point.z = safe(z);
|
||||
if (sp_type == POSITION || sp_type == NAVIGATE || sp_type == NAVIGATE_GLOBAL || sp_type == VELOCITY || sp_type == ATTITUDE) {
|
||||
// destination point and/or yaw
|
||||
PoseStamped ps;
|
||||
ps.header.frame_id = frame_id;
|
||||
ps.header.stamp = stamp;
|
||||
ps.pose.position.x = x;
|
||||
ps.pose.position.y = y;
|
||||
ps.pose.position.z = z;
|
||||
ps.pose.orientation.w = 1.0; // Ensure quaternion is always valid
|
||||
|
||||
// transform to reference frame
|
||||
desired = tf_buffer.transform(desired, reference_frame);
|
||||
|
||||
// set horizontal position
|
||||
if (isfinite(x) && isfinite(y)) {
|
||||
setpoint_position = desired;
|
||||
} else if (setpoint_position.header.frame_id.empty()) {
|
||||
// TODO: use transform for current stamp
|
||||
setpoint_position.header = local_position.header;
|
||||
setpoint_position.point = local_position.pose.position;
|
||||
}
|
||||
|
||||
// set altitude
|
||||
if (isfinite(z)) {
|
||||
setpoint_altitude = desired;
|
||||
} else if (setpoint_altitude.header.frame_id.empty()) {
|
||||
setpoint_altitude.header = local_position.header;
|
||||
setpoint_altitude.point = local_position.pose.position;
|
||||
}
|
||||
}
|
||||
|
||||
// handle velocity
|
||||
if (sp_type == VELOCITY) {
|
||||
// TODO: allow setting different modes by altitude and xy
|
||||
Vector3Stamped desired;
|
||||
desired.header.frame_id = frame_id;
|
||||
desired.header.stamp = stamp;
|
||||
desired.vector.x = safe(vx);
|
||||
desired.vector.y = safe(vy);
|
||||
desired.vector.z = safe(vz);
|
||||
|
||||
// transform to reference frame
|
||||
desired = tf_buffer.transform(desired, reference_frame);
|
||||
setpoint_velocity.header = desired.header;
|
||||
|
||||
// set horizontal velocity
|
||||
if (isfinite(vx) && isfinite(vy)) {
|
||||
setpoint_velocity.vector.x = desired.vector.x;
|
||||
setpoint_velocity.vector.y = desired.vector.y;
|
||||
}
|
||||
|
||||
// set vertical velocity
|
||||
if (isfinite(vz)) {
|
||||
setpoint_velocity.vector.z = desired.vector.z;
|
||||
}
|
||||
}
|
||||
|
||||
// handle yaw
|
||||
if (sp_type == NAVIGATE || sp_type == NAVIGATE_GLOBAL || sp_type == POSITION || sp_type == VELOCITY || sp_type == ATTITUDE || sp_type == _YAW) {
|
||||
if (isfinite(yaw)) {
|
||||
setpoint_yaw_type = YAW;
|
||||
QuaternionStamped desired;
|
||||
desired.header.frame_id = frame_id;
|
||||
desired.header.stamp = stamp;
|
||||
desired.quaternion = tf::createQuaternionMsgFromYaw(yaw);
|
||||
|
||||
// transform to reference frame
|
||||
desired = tf_buffer.transform(desired, reference_frame);
|
||||
setpoint_yaw = tf2::getYaw(desired.quaternion);
|
||||
yaw_frame_id = reference_frame;
|
||||
|
||||
} else if (isinf(yaw) && yaw > 0) {
|
||||
if (std::isnan(yaw)) {
|
||||
setpoint_yaw_type = YAW_RATE;
|
||||
setpoint_yaw_rate = yaw_rate;
|
||||
} else if (std::isinf(yaw) && yaw > 0) {
|
||||
// yaw towards
|
||||
setpoint_yaw_type = TOWARDS;
|
||||
|
||||
} else if (yaw_frame_id.empty() || sp_type == _YAW) {
|
||||
// yaw is nan and not set previously OR set_yaw(yaw=nan) was called
|
||||
yaw = 0;
|
||||
setpoint_yaw_rate = 0;
|
||||
} else {
|
||||
setpoint_yaw_type = YAW;
|
||||
setpoint_yaw = tf2::getYaw(local_position.pose.orientation); // set yaw to current yaw
|
||||
yaw_frame_id = local_position.header.frame_id;
|
||||
setpoint_yaw_rate = 0;
|
||||
ps.pose.orientation = tf::createQuaternionMsgFromYaw(yaw);
|
||||
}
|
||||
|
||||
tf_buffer.transform(ps, setpoint_position, reference_frame);
|
||||
}
|
||||
|
||||
// handle roll
|
||||
if (isfinite(roll)) {
|
||||
setpoint_roll = roll;
|
||||
if (sp_type == VELOCITY) {
|
||||
Vector3Stamped vel;
|
||||
vel.header.frame_id = frame_id;
|
||||
vel.header.stamp = stamp;
|
||||
vel.vector.x = vx;
|
||||
vel.vector.y = vy;
|
||||
vel.vector.z = vz;
|
||||
tf_buffer.transform(vel, setpoint_velocity, reference_frame);
|
||||
}
|
||||
|
||||
// handle pitch
|
||||
if (isfinite(pitch)) {
|
||||
setpoint_pitch = pitch;
|
||||
if (sp_type == ATTITUDE || sp_type == RATES) {
|
||||
thrust_msg.thrust = thrust;
|
||||
}
|
||||
|
||||
// handle yaw rate
|
||||
if (isfinite(yaw_rate)) {
|
||||
setpoint_yaw_type = YAW_RATE;
|
||||
setpoint_rates.z = yaw_rate;
|
||||
}
|
||||
|
||||
// handle pitch rate
|
||||
if (isfinite(roll_rate)) {
|
||||
setpoint_rates.x = roll_rate;
|
||||
}
|
||||
|
||||
// handle roll rate
|
||||
if (isfinite(pitch_rate)) {
|
||||
setpoint_rates.y = pitch_rate;
|
||||
}
|
||||
|
||||
// handle thrust
|
||||
if (isfinite(thrust)) {
|
||||
setpoint_thrust = thrust;
|
||||
if (sp_type == RATES) {
|
||||
rates_msg.twist.angular.x = roll_rate;
|
||||
rates_msg.twist.angular.y = pitch_rate;
|
||||
rates_msg.twist.angular.z = yaw_rate;
|
||||
}
|
||||
|
||||
wait_armed = auto_arm;
|
||||
|
||||
publish_setpoint:
|
||||
publish(stamp); // calculate initial transformed messages first
|
||||
setpoint_timer.start();
|
||||
|
||||
if (setpoint_type == NAVIGATE || setpoint_type == NAVIGATE_GLOBAL || setpoint_type == POSITION) {
|
||||
publishTarget(stamp, true);
|
||||
// publish target frame
|
||||
if (!target.child_frame_id.empty()) {
|
||||
if (setpoint_type == NAVIGATE || setpoint_type == NAVIGATE_GLOBAL || setpoint_type == POSITION) {
|
||||
target.header.frame_id = setpoint_position.header.frame_id;
|
||||
target.header.stamp = stamp;
|
||||
target.transform.translation.x = setpoint_position.pose.position.x;
|
||||
target.transform.translation.y = setpoint_position.pose.position.y;
|
||||
target.transform.translation.z = setpoint_position.pose.position.z;
|
||||
target.transform.rotation = setpoint_position.pose.orientation;
|
||||
static_transform_broadcaster->sendTransform(target);
|
||||
}
|
||||
}
|
||||
|
||||
publishState();
|
||||
|
||||
if (auto_arm) {
|
||||
offboardAndArm();
|
||||
wait_armed = false;
|
||||
@@ -984,39 +778,27 @@ bool serve(enum setpoint_type_t sp_type, float x, float y, float z, float vx, fl
|
||||
}
|
||||
|
||||
bool navigate(Navigate::Request& req, Navigate::Response& res) {
|
||||
return serve(NAVIGATE, req.x, req.y, req.z, NAN, NAN, NAN, NAN, NAN, req.yaw, NAN, NAN, NAN, NAN, NAN, NAN, req.speed, req.frame_id, req.auto_arm, res.success, res.message);
|
||||
return serve(NAVIGATE, req.x, req.y, req.z, NAN, NAN, NAN, NAN, NAN, req.yaw, NAN, NAN, req.yaw_rate, NAN, NAN, NAN, req.speed, req.frame_id, req.auto_arm, res.success, res.message);
|
||||
}
|
||||
|
||||
bool navigateGlobal(NavigateGlobal::Request& req, NavigateGlobal::Response& res) {
|
||||
return serve(NAVIGATE_GLOBAL, NAN, NAN, req.z, NAN, NAN, NAN, NAN, NAN, req.yaw, NAN, NAN, NAN, req.lat, req.lon, NAN, req.speed, req.frame_id, req.auto_arm, res.success, res.message);
|
||||
}
|
||||
|
||||
bool setAltitude(SetAltitude::Request& req, SetAltitude::Response& res) {
|
||||
return serve(_ALTITUDE, NAN, NAN, req.z, NAN, NAN, NAN, NAN, NAN, NAN, NAN, NAN, NAN, NAN, NAN, NAN, NAN, req.frame_id, false, res.success, res.message);
|
||||
}
|
||||
|
||||
bool setYaw(SetYaw::Request& req, SetYaw::Response& res) {
|
||||
return serve(_YAW, NAN, NAN, NAN, NAN, NAN, NAN, NAN, NAN, req.yaw, NAN, NAN, NAN, NAN, NAN, NAN, NAN, req.frame_id, false, res.success, res.message);
|
||||
}
|
||||
|
||||
bool setYawRate(SetYawRate::Request& req, SetYawRate::Response& res) {
|
||||
return serve(_YAW_RATE, NAN, NAN, NAN, NAN, NAN, NAN, NAN, NAN, NAN, NAN, NAN, req.yaw_rate, NAN, NAN, NAN, NAN, "", false, res.success, res.message);
|
||||
return serve(NAVIGATE_GLOBAL, NAN, NAN, req.z, NAN, NAN, NAN, NAN, NAN, req.yaw, NAN, NAN, req.yaw_rate, req.lat, req.lon, NAN, req.speed, req.frame_id, req.auto_arm, res.success, res.message);
|
||||
}
|
||||
|
||||
bool setPosition(SetPosition::Request& req, SetPosition::Response& res) {
|
||||
return serve(POSITION, req.x, req.y, req.z, NAN, NAN, NAN, NAN, NAN, req.yaw, NAN, NAN, NAN, NAN, NAN, NAN, NAN, req.frame_id, req.auto_arm, res.success, res.message);
|
||||
return serve(POSITION, req.x, req.y, req.z, NAN, NAN, NAN, NAN, NAN, req.yaw, NAN, NAN, req.yaw_rate, NAN, NAN, NAN, NAN, req.frame_id, req.auto_arm, res.success, res.message);
|
||||
}
|
||||
|
||||
bool setVelocity(SetVelocity::Request& req, SetVelocity::Response& res) {
|
||||
return serve(VELOCITY, NAN, NAN, NAN, req.vx, req.vy, req.vz, NAN, NAN, req.yaw, NAN, NAN, NAN, NAN, NAN, NAN, NAN, req.frame_id, req.auto_arm, res.success, res.message);
|
||||
return serve(VELOCITY, NAN, NAN, NAN, req.vx, req.vy, req.vz, NAN, NAN, req.yaw, NAN, NAN, req.yaw_rate, NAN, NAN, NAN, NAN, req.frame_id, req.auto_arm, res.success, res.message);
|
||||
}
|
||||
|
||||
bool setAttitude(SetAttitude::Request& req, SetAttitude::Response& res) {
|
||||
return serve(ATTITUDE, NAN, NAN, NAN, NAN, NAN, NAN, req.roll, req.pitch, req.yaw, NAN, NAN, NAN, NAN, NAN, req.thrust, NAN, req.frame_id, req.auto_arm, res.success, res.message);
|
||||
return serve(ATTITUDE, NAN, NAN, NAN, NAN, NAN, NAN, req.pitch, req.roll, req.yaw, NAN, NAN, NAN, NAN, NAN, req.thrust, NAN, req.frame_id, req.auto_arm, res.success, res.message);
|
||||
}
|
||||
|
||||
bool setRates(SetRates::Request& req, SetRates::Response& res) {
|
||||
return serve(RATES, NAN, NAN, NAN, NAN, NAN, NAN, NAN, NAN, NAN, req.roll_rate, req.pitch_rate, req.yaw_rate, NAN, NAN, req.thrust, NAN, "", req.auto_arm, res.success, res.message);
|
||||
return serve(RATES, NAN, NAN, NAN, NAN, NAN, NAN, NAN, NAN, NAN, req.pitch_rate, req.roll_rate, req.yaw_rate, NAN, NAN, req.thrust, NAN, "", req.auto_arm, res.success, res.message);
|
||||
}
|
||||
|
||||
bool land(std_srvs::Trigger::Request& req, std_srvs::Trigger::Response& res)
|
||||
@@ -1048,7 +830,9 @@ bool land(std_srvs::Trigger::Request& req, std_srvs::Trigger::Response& res)
|
||||
auto start = ros::Time::now();
|
||||
while (ros::ok()) {
|
||||
if (state.mode == "AUTO.LAND") {
|
||||
break;
|
||||
res.success = true;
|
||||
busy = false;
|
||||
return true;
|
||||
}
|
||||
if (ros::Time::now() - start > land_timeout)
|
||||
throw std::runtime_error("Land request timed out");
|
||||
@@ -1057,37 +841,12 @@ bool land(std_srvs::Trigger::Request& req, std_srvs::Trigger::Response& res)
|
||||
r.sleep();
|
||||
}
|
||||
|
||||
// stop setpoints and invalidate position setpoint
|
||||
setpoint_timer.stop();
|
||||
setpoint_type = NONE;
|
||||
setpoint_position.header.frame_id = "";
|
||||
setpoint_altitude.header.frame_id = "";
|
||||
yaw_frame_id = "";
|
||||
publishState();
|
||||
|
||||
res.success = true;
|
||||
busy = false;
|
||||
return true;
|
||||
|
||||
} catch (const std::exception& e) {
|
||||
res.message = e.what();
|
||||
ROS_INFO("%s", e.what());
|
||||
busy = false;
|
||||
return true;
|
||||
}
|
||||
return false;
|
||||
}
|
||||
|
||||
bool release(std_srvs::Trigger::Request& req, std_srvs::Trigger::Response& res)
|
||||
{
|
||||
setpoint_timer.stop();
|
||||
setpoint_type = NONE;
|
||||
setpoint_position.header.frame_id = "";
|
||||
setpoint_altitude.header.frame_id = "";
|
||||
yaw_frame_id = "";
|
||||
publishState();
|
||||
res.success = true;
|
||||
return true;
|
||||
}
|
||||
|
||||
int main(int argc, char **argv)
|
||||
@@ -1100,9 +859,8 @@ int main(int argc, char **argv)
|
||||
static_transform_broadcaster = std::make_shared<tf2_ros::StaticTransformBroadcaster>();
|
||||
|
||||
// Params
|
||||
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.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);
|
||||
@@ -1111,17 +869,8 @@ int main(int argc, char **argv)
|
||||
nh_priv.param("check_kill_switch", check_kill_switch, true);
|
||||
nh_priv.param("default_speed", default_speed, 0.5f);
|
||||
nh_priv.param<string>("body_frame", body.child_frame_id, "body");
|
||||
nh_priv.param<string>("terrain_frame", terrain.child_frame_id, "terrain");
|
||||
nh_priv.param<string>("terrain_frame_mode", terrain_frame_mode, "altitude");
|
||||
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));
|
||||
@@ -1136,56 +885,35 @@ 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);
|
||||
|
||||
ros::Subscriber altitude_sub;
|
||||
if (!body.child_frame_id.empty() && !terrain.child_frame_id.empty()) {
|
||||
terrain.header.frame_id = local_frame;
|
||||
if (terrain_frame_mode == "altitude") {
|
||||
altitude_sub = nh.subscribe(mavros + "/altitude", 1, &handleAltitude);
|
||||
} else if (terrain_frame_mode == "range") {
|
||||
string range_topic = nh_priv.param("range_topic", string("rangefinder/range"));
|
||||
altitude_sub = nh.subscribe(range_topic, 1, &handleRange);
|
||||
} else {
|
||||
ROS_FATAL("Unknown terrain_frame_mode: %s, valid values: altitude, range", terrain_frame_mode.c_str());
|
||||
ros::shutdown();
|
||||
}
|
||||
}
|
||||
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);
|
||||
|
||||
// State publisher
|
||||
state_pub = nh_priv.advertise<clover::State>("state", 1, true);
|
||||
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);
|
||||
auto na_serv = nh.advertiseService("navigate", &navigate);
|
||||
auto ng_serv = nh.advertiseService("navigate_global", &navigateGlobal);
|
||||
auto sl_serv = nh.advertiseService("set_altitude", &setAltitude);
|
||||
auto ya_serv = nh.advertiseService("set_yaw", &setYaw);
|
||||
auto yr_serv = nh.advertiseService("set_yaw_rate", &setYawRate);
|
||||
auto sp_serv = nh.advertiseService("set_position", &setPosition);
|
||||
auto sv_serv = nh.advertiseService("set_velocity", &setVelocity);
|
||||
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);
|
||||
@@ -1193,7 +921,7 @@ int main(int argc, char **argv)
|
||||
position_msg.header.frame_id = local_frame;
|
||||
position_raw_msg.header.frame_id = local_frame;
|
||||
position_raw_msg.coordinate_frame = PositionTarget::FRAME_LOCAL_NED;
|
||||
//rates_msg.header.frame_id = fcu_frame;
|
||||
rates_msg.header.frame_id = fcu_frame;
|
||||
|
||||
ROS_INFO("ready");
|
||||
ros::spin();
|
||||
|
||||
@@ -11,14 +11,12 @@
|
||||
|
||||
#include <string>
|
||||
#include <ros/ros.h>
|
||||
#include <tf/transform_datatypes.h>
|
||||
#include <tf2/transform_datatypes.h>
|
||||
#include <tf2_ros/buffer.h>
|
||||
#include <tf2_ros/transform_listener.h>
|
||||
#include <tf2_ros/static_transform_broadcaster.h>
|
||||
#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
|
||||
#include <geometry_msgs/TransformStamped.h>
|
||||
#include <geometry_msgs/Quaternion.h>
|
||||
#include <geometry_msgs/PoseStamped.h>
|
||||
#include <geometry_msgs/PoseWithCovarianceStamped.h>
|
||||
#include <std_srvs/Trigger.h>
|
||||
@@ -27,7 +25,7 @@
|
||||
using std::string;
|
||||
using namespace geometry_msgs;
|
||||
|
||||
bool reset_flag = true; // offset should be reset on the start
|
||||
bool reset_flag = false;
|
||||
string local_frame_id, frame_id, child_frame_id, offset_frame_id;
|
||||
tf2_ros::Buffer tf_buffer;
|
||||
ros::Publisher vpe_pub;
|
||||
@@ -35,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_timeout, publish_zero_duration, offset_timeout;
|
||||
ros::Duration publish_zero_timout, publish_zero_duration, offset_timeout;
|
||||
TransformStamped offset;
|
||||
|
||||
void publishZero(const ros::TimerEvent& e)
|
||||
{
|
||||
if (!vpe.header.stamp.isZero() && e.current_real - vpe.header.stamp < publish_zero_timeout) return; // have vpe
|
||||
if (e.current_real - vpe.header.stamp < publish_zero_timout) return; // have vpe
|
||||
|
||||
if (!pose.header.stamp.isZero() && e.current_real - pose.header.stamp < publish_zero_timeout) { // have local position
|
||||
if (e.current_real - pose.header.stamp < publish_zero_timout) { // have local position
|
||||
if (got_local_pos.isZero()) {
|
||||
ROS_INFO("got local position");
|
||||
got_local_pos = e.current_real;
|
||||
@@ -68,13 +66,6 @@ inline Pose getPose(const PoseStampedConstPtr& pose) { return pose->pose; }
|
||||
|
||||
inline Pose getPose(const PoseWithCovarianceStampedConstPtr& pose) { return pose->pose.pose; }
|
||||
|
||||
inline void keepYaw(Quaternion& quaternion)
|
||||
{
|
||||
tf::Quaternion q;
|
||||
q.setRPY(0, 0, tf::getYaw(quaternion));
|
||||
tf::quaternionTFToMsg(q, quaternion);
|
||||
}
|
||||
|
||||
template <typename T>
|
||||
void callback(const T& msg)
|
||||
{
|
||||
@@ -97,29 +88,10 @@ void callback(const T& msg)
|
||||
if (!offset_frame_id.empty()) {
|
||||
if (reset_flag || msg->header.stamp - vpe.header.stamp > offset_timeout) {
|
||||
// calculate the offset
|
||||
if (!frame_id.empty()) {
|
||||
// calculate from TF
|
||||
offset = tf_buffer.lookupTransform(local_frame_id, frame_id,
|
||||
msg->header.stamp, ros::Duration(0.02));
|
||||
// offset.header.frame_id = vpe.header.frame_id;
|
||||
offset.child_frame_id = offset_frame_id;
|
||||
|
||||
} else {
|
||||
// calculate transform between pose in vpe frame and pose in local frame
|
||||
TransformStamped local_pose = tf_buffer.lookupTransform(local_frame_id, child_frame_id,
|
||||
msg->header.stamp, ros::Duration(0.02));
|
||||
keepYaw(local_pose.transform.rotation);
|
||||
|
||||
tf::Transform vpeTransform, poseTransform;
|
||||
tf::poseMsgToTF(vpe.pose, vpeTransform);
|
||||
tf::transformMsgToTF(local_pose.transform, poseTransform);
|
||||
tf::Transform offset_tf = vpeTransform.inverseTimes(poseTransform);
|
||||
tf::transformTFToMsg(offset_tf, offset.transform);
|
||||
offset.header.frame_id = local_frame_id;
|
||||
offset.header.stamp = msg->header.stamp;
|
||||
offset.child_frame_id = offset_frame_id;
|
||||
}
|
||||
|
||||
offset = tf_buffer.lookupTransform(local_frame_id, frame_id,
|
||||
msg->header.stamp, ros::Duration(0.02));
|
||||
// offset.header.frame_id = vpe.header.frame_id;
|
||||
offset.child_frame_id = offset_frame_id;
|
||||
br.sendTransform(offset);
|
||||
reset_flag = false;
|
||||
ROS_INFO("offset reset");
|
||||
@@ -150,11 +122,10 @@ int main(int argc, char **argv) {
|
||||
|
||||
tf2_ros::TransformListener tf_listener(tf_buffer);
|
||||
|
||||
nh_priv.param<string>("frame_id", frame_id, ""); // name for used visual pose frame
|
||||
nh_priv.param<string>("offset_frame_id", offset_frame_id, ""); // name for published offset frame
|
||||
|
||||
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");
|
||||
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");
|
||||
offset_timeout = ros::Duration(nh_priv.param("offset_timeout", 3.0));
|
||||
|
||||
if (!frame_id.empty()) {
|
||||
@@ -170,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("force_init", false) || nh_priv.param("publish_zero", false)) { // publish_zero is old name
|
||||
if (nh_priv.param("publish_zero", false)) {
|
||||
// publish zero to initialize the local position
|
||||
zero_timer = nh.createTimer(ros::Duration(0.1), &publishZero);
|
||||
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));
|
||||
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));
|
||||
local_position_sub = nh.subscribe("mavros/local_position/pose", 1, &localPositionCallback);
|
||||
}
|
||||
|
||||
|
||||
@@ -1,4 +0,0 @@
|
||||
#!/usr/bin/env bash
|
||||
|
||||
export ROSWWW_DEFAULT=clover
|
||||
rosrun roswww_static update
|
||||
@@ -13,11 +13,11 @@ float32 alt
|
||||
float32 vx
|
||||
float32 vy
|
||||
float32 vz
|
||||
float32 roll
|
||||
float32 pitch
|
||||
float32 roll
|
||||
float32 yaw
|
||||
float32 roll_rate
|
||||
float32 pitch_rate
|
||||
float32 roll_rate
|
||||
float32 yaw_rate
|
||||
float32 voltage
|
||||
float32 cell_voltage
|
||||
|
||||
@@ -2,6 +2,7 @@ float32 x
|
||||
float32 y
|
||||
float32 z
|
||||
float32 yaw
|
||||
float32 yaw_rate
|
||||
float32 speed
|
||||
string frame_id
|
||||
bool auto_arm
|
||||
|
||||
@@ -2,6 +2,7 @@ float64 lat
|
||||
float64 lon
|
||||
float32 z
|
||||
float32 yaw
|
||||
float32 yaw_rate
|
||||
float32 speed
|
||||
string frame_id
|
||||
bool auto_arm
|
||||
|
||||
@@ -1,5 +0,0 @@
|
||||
float32 z
|
||||
string frame_id
|
||||
---
|
||||
bool success
|
||||
string message
|
||||
@@ -1,5 +1,5 @@
|
||||
float32 roll
|
||||
float32 pitch
|
||||
float32 roll
|
||||
float32 yaw
|
||||
float32 thrust
|
||||
string frame_id
|
||||
|
||||
@@ -2,6 +2,7 @@ float32 x
|
||||
float32 y
|
||||
float32 z
|
||||
float32 yaw
|
||||
float32 yaw_rate
|
||||
string frame_id
|
||||
bool auto_arm
|
||||
---
|
||||
|
||||
@@ -1,5 +1,5 @@
|
||||
float32 roll_rate
|
||||
float32 pitch_rate
|
||||
float32 roll_rate
|
||||
float32 yaw_rate
|
||||
float32 thrust
|
||||
bool auto_arm
|
||||
|
||||
@@ -2,6 +2,7 @@ float32 vx
|
||||
float32 vy
|
||||
float32 vz
|
||||
float32 yaw
|
||||
float32 yaw_rate
|
||||
string frame_id
|
||||
bool auto_arm
|
||||
---
|
||||
|
||||
@@ -1,5 +0,0 @@
|
||||
float32 yaw
|
||||
string frame_id
|
||||
---
|
||||
bool success
|
||||
string message
|
||||
@@ -1,4 +0,0 @@
|
||||
float32 yaw_rate
|
||||
---
|
||||
bool success
|
||||
string message
|
||||
@@ -3,7 +3,6 @@ import rospy
|
||||
import pytest
|
||||
from mavros_msgs.msg import State
|
||||
from clover import srv
|
||||
import time
|
||||
|
||||
@pytest.fixture()
|
||||
def node():
|
||||
@@ -25,7 +24,6 @@ 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:
|
||||
@@ -35,44 +33,3 @@ 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
|
||||
|
||||
@@ -23,7 +23,10 @@
|
||||
|
||||
<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"/>
|
||||
<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="tf2_web_republisher" pkg="tf2_web_republisher" type="tf2_web_republisher" required="true"/>
|
||||
|
||||
@@ -35,11 +38,6 @@
|
||||
<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>
|
||||
|
||||
@@ -1,437 +0,0 @@
|
||||
import rospy
|
||||
import pytest
|
||||
from pytest import approx
|
||||
import threading
|
||||
import mavros_msgs.msg
|
||||
from mavros_msgs.srv import SetMode
|
||||
from geometry_msgs.msg import PoseStamped
|
||||
from clover import srv
|
||||
from clover.msg import State
|
||||
from std_srvs.srv import Trigger
|
||||
from math import nan, inf
|
||||
import tf2_ros
|
||||
import tf2_geometry_msgs
|
||||
|
||||
@pytest.fixture()
|
||||
def node():
|
||||
return rospy.init_node('offboard_test', anonymous=True)
|
||||
|
||||
@pytest.fixture
|
||||
def tf_buffer():
|
||||
buf = tf2_ros.Buffer()
|
||||
tf2_ros.TransformListener(buf)
|
||||
return buf
|
||||
|
||||
def get_state():
|
||||
return rospy.wait_for_message('/simple_offboard/state', State, timeout=1)
|
||||
|
||||
def get_navigate_target(tf_buffer):
|
||||
target = tf_buffer.lookup_transform('map', 'navigate_target', rospy.get_rostime(), rospy.Duration(1))
|
||||
assert target.child_frame_id == 'navigate_target'
|
||||
return target
|
||||
|
||||
def test_offboard(node, tf_buffer):
|
||||
navigate = rospy.ServiceProxy('navigate', srv.Navigate)
|
||||
set_position = rospy.ServiceProxy('set_position', srv.SetPosition)
|
||||
set_altitude = rospy.ServiceProxy('set_altitude', srv.SetAltitude)
|
||||
set_yaw = rospy.ServiceProxy('set_yaw', srv.SetYaw)
|
||||
set_yaw_rate = rospy.ServiceProxy('set_yaw_rate', srv.SetYawRate)
|
||||
set_velocity = rospy.ServiceProxy('set_velocity', srv.SetVelocity)
|
||||
set_attitude = rospy.ServiceProxy('set_attitude', srv.SetAttitude)
|
||||
set_rates = rospy.ServiceProxy('set_rates', srv.SetRates)
|
||||
get_telemetry = rospy.ServiceProxy('get_telemetry', srv.GetTelemetry)
|
||||
land = rospy.ServiceProxy('land', Trigger)
|
||||
|
||||
res = navigate()
|
||||
assert res.success == False
|
||||
assert res.message.startswith('State timeout')
|
||||
|
||||
telem = get_telemetry()
|
||||
assert telem.connected == False
|
||||
|
||||
# mocked state publisher
|
||||
state_pub = rospy.Publisher('/mavros/state', mavros_msgs.msg.State, latch=True, queue_size=1)
|
||||
state_msg = mavros_msgs.msg.State(mode='OFFBOARD', armed=True)
|
||||
|
||||
def publish_state():
|
||||
r = rospy.Rate(2)
|
||||
while not rospy.is_shutdown():
|
||||
state_msg.header.stamp = rospy.Time.now()
|
||||
state_pub.publish(state_msg)
|
||||
r.sleep()
|
||||
|
||||
# start publishing state
|
||||
threading.Thread(target=publish_state, daemon=True).start()
|
||||
rospy.sleep(0.5)
|
||||
|
||||
# set_mode service mock
|
||||
def set_mode(req):
|
||||
state_msg.mode = req.custom_mode # set mocked mode to requested
|
||||
return True,
|
||||
|
||||
rospy.Service('/mavros/set_mode', SetMode, set_mode)
|
||||
|
||||
telem = get_telemetry()
|
||||
assert telem.connected == False
|
||||
|
||||
res = navigate()
|
||||
assert res.success == False
|
||||
assert res.message.startswith('No connection to FCU')
|
||||
|
||||
state_msg.connected = True
|
||||
rospy.sleep(1)
|
||||
|
||||
telem = get_telemetry()
|
||||
assert telem.connected == True
|
||||
|
||||
res = navigate()
|
||||
assert res.success == False
|
||||
assert res.message.startswith('No local position')
|
||||
|
||||
local_position_pub = rospy.Publisher('/mavros/local_position/pose', PoseStamped, latch=True, queue_size=1)
|
||||
local_position_msg = PoseStamped()
|
||||
local_position_msg.header.frame_id = 'map'
|
||||
local_position_msg.pose.position.x = 1
|
||||
local_position_msg.pose.position.y = 2
|
||||
local_position_msg.pose.position.z = 3
|
||||
local_position_msg.pose.orientation.w = 1
|
||||
|
||||
def publish_local_position():
|
||||
r = rospy.Rate(30)
|
||||
while not rospy.is_shutdown():
|
||||
local_position_msg.header.stamp = rospy.Time.now()
|
||||
local_position_pub.publish(local_position_msg)
|
||||
r.sleep()
|
||||
|
||||
# start publishing local position
|
||||
threading.Thread(target=publish_local_position, daemon=True).start()
|
||||
rospy.sleep(0.5)
|
||||
|
||||
# check body frame
|
||||
body = tf_buffer.lookup_transform('map', 'body', rospy.get_rostime(), rospy.Duration(1))
|
||||
assert body.child_frame_id == 'body'
|
||||
assert body.transform.translation.x == approx(1)
|
||||
assert body.transform.translation.y == approx(2)
|
||||
assert body.transform.translation.z == approx(3)
|
||||
|
||||
res = navigate(x=3, y=2, z=1, frame_id='map')
|
||||
assert res.success == True
|
||||
state = get_state()
|
||||
assert state.mode == State.MODE_NAVIGATE
|
||||
assert state.yaw_mode == State.YAW_MODE_YAW
|
||||
assert state.x == 3
|
||||
assert state.y == 2
|
||||
assert state.z == 1
|
||||
assert state.yaw == 0
|
||||
assert state.xy_frame_id == 'map'
|
||||
assert state.z_frame_id == 'map'
|
||||
assert state.yaw_frame_id == 'map'
|
||||
target = get_navigate_target(tf_buffer)
|
||||
assert target.header.frame_id == 'map'
|
||||
assert target.transform.translation.x == approx(3)
|
||||
assert target.transform.translation.y == approx(2)
|
||||
assert target.transform.translation.z == approx(1)
|
||||
assert target.transform.rotation.x == 0
|
||||
assert target.transform.rotation.y == 0
|
||||
assert target.transform.rotation.z == 0
|
||||
assert target.transform.rotation.w == 1
|
||||
|
||||
# try to set only the y
|
||||
res = navigate(x=nan, y=1, z=nan)
|
||||
assert res.success == False
|
||||
assert res.message.startswith('x and y can be set only together')
|
||||
|
||||
# set z in body frame
|
||||
res = navigate(x=nan, y=nan, z=1, frame_id='body')
|
||||
assert res.success == True
|
||||
state = get_state()
|
||||
assert state.mode == State.MODE_NAVIGATE
|
||||
assert state.yaw_mode == State.YAW_MODE_YAW
|
||||
assert state.x == 3
|
||||
assert state.y == 2
|
||||
assert state.z == 4
|
||||
assert state.yaw == 0
|
||||
assert state.xy_frame_id == 'map'
|
||||
assert state.z_frame_id == 'map'
|
||||
assert state.yaw_frame_id == 'map'
|
||||
|
||||
# set xy in test frame
|
||||
res = navigate(x=1, y=2, z=nan, frame_id='test')
|
||||
assert res.success == True
|
||||
state = get_state()
|
||||
assert state.mode == State.MODE_NAVIGATE
|
||||
assert state.yaw_mode == State.YAW_MODE_YAW
|
||||
assert state.x == 1
|
||||
assert state.y == 2
|
||||
assert state.z == 4
|
||||
assert state.yaw == 0
|
||||
assert state.xy_frame_id == 'test'
|
||||
assert state.z_frame_id == 'map'
|
||||
assert state.yaw_frame_id == 'test'
|
||||
|
||||
# auto_arm should not invalidate the setpoint if not effective
|
||||
res = navigate(x=nan, y=nan, z=1, frame_id='map', auto_arm=True)
|
||||
assert res.success == True
|
||||
state = get_state()
|
||||
assert state.mode == State.MODE_NAVIGATE
|
||||
assert state.yaw_mode == State.YAW_MODE_YAW
|
||||
assert state.x == 1
|
||||
assert state.y == 2
|
||||
assert state.z == 1
|
||||
assert state.yaw == 0
|
||||
assert state.xy_frame_id == 'test'
|
||||
assert state.z_frame_id == 'map'
|
||||
assert state.yaw_frame_id == 'map'
|
||||
|
||||
# auto_arm should invalidate the setpoint if effective
|
||||
state_msg.mode = 'STABILIZED' # pretend we are not in OFFBOARD mode
|
||||
rospy.sleep(1)
|
||||
res = navigate(x=nan, y=nan, z=1, frame_id='map', auto_arm=True)
|
||||
assert res.success == True
|
||||
state = get_state()
|
||||
assert state.mode == State.MODE_NAVIGATE
|
||||
assert state.yaw_mode == State.YAW_MODE_YAW
|
||||
assert state.x == 1
|
||||
assert state.y == 2
|
||||
assert state.z == 1
|
||||
assert state.yaw == 0
|
||||
assert state.xy_frame_id == 'map'
|
||||
assert state.z_frame_id == 'map'
|
||||
assert state.yaw_frame_id == 'map'
|
||||
state_msg.mode = 'OFFBOARD'
|
||||
rospy.sleep(1)
|
||||
|
||||
# set_attitude should invalidate the setpoint
|
||||
res = set_attitude()
|
||||
assert res.success == True
|
||||
|
||||
res = navigate(x=5, y=6, z=nan, yaw=nan, frame_id='map')
|
||||
assert res.success == True
|
||||
state = get_state()
|
||||
assert state.mode == State.MODE_NAVIGATE
|
||||
assert state.yaw_mode == State.YAW_MODE_YAW
|
||||
assert state.x == 5
|
||||
assert state.y == 6
|
||||
assert state.z == 3
|
||||
assert state.yaw == 0
|
||||
assert state.xy_frame_id == 'map'
|
||||
assert state.z_frame_id == 'map'
|
||||
assert state.yaw_frame_id == 'map'
|
||||
|
||||
# test set_altitude
|
||||
res = set_altitude(z=7, frame_id='test')
|
||||
assert res.success == True
|
||||
state = get_state()
|
||||
assert state.mode == State.MODE_NAVIGATE
|
||||
assert state.yaw_mode == State.YAW_MODE_YAW
|
||||
assert state.x == 5
|
||||
assert state.y == 6
|
||||
assert state.z == 7
|
||||
assert state.yaw == 0
|
||||
assert state.xy_frame_id == 'map'
|
||||
assert state.z_frame_id == 'test'
|
||||
assert state.yaw_frame_id == 'map'
|
||||
|
||||
# test set_yaw
|
||||
res = set_yaw(yaw=0.5, frame_id='test2')
|
||||
assert res.success == True
|
||||
state = get_state()
|
||||
assert state.mode == State.MODE_NAVIGATE
|
||||
assert state.yaw_mode == State.YAW_MODE_YAW
|
||||
assert state.x == 5
|
||||
assert state.y == 6
|
||||
assert state.z == 7
|
||||
assert state.yaw == 0.5
|
||||
assert state.xy_frame_id == 'map'
|
||||
assert state.z_frame_id == 'test'
|
||||
assert state.yaw_frame_id == 'test2'
|
||||
|
||||
# test set_yaw_rate
|
||||
res = set_yaw_rate(yaw_rate=2)
|
||||
assert res.success == True
|
||||
state = get_state()
|
||||
assert state.mode == State.MODE_NAVIGATE
|
||||
assert state.yaw_mode == State.YAW_MODE_YAW_RATE
|
||||
assert state.x == 5
|
||||
assert state.y == 6
|
||||
assert state.z == 7
|
||||
assert state.yaw_rate == 2
|
||||
assert state.xy_frame_id == 'map'
|
||||
assert state.z_frame_id == 'test'
|
||||
|
||||
# navigate(yaw=nan) should keep yaw rate mode
|
||||
res = navigate(x=nan, y=nan, z=nan, yaw=nan)
|
||||
assert res.success == True
|
||||
state = get_state()
|
||||
assert state.mode == State.MODE_NAVIGATE
|
||||
assert state.yaw_mode == State.YAW_MODE_YAW_RATE
|
||||
assert state.x == 5
|
||||
assert state.y == 6
|
||||
assert state.z == 7
|
||||
assert state.yaw_rate == 2
|
||||
assert state.xy_frame_id == 'map'
|
||||
assert state.z_frame_id == 'test'
|
||||
|
||||
# set_yaw(nan) should change back to yaw mode
|
||||
res = set_yaw(yaw=nan)
|
||||
assert res.success == True
|
||||
state = get_state()
|
||||
assert state.mode == State.MODE_NAVIGATE
|
||||
assert state.yaw_mode == State.YAW_MODE_YAW
|
||||
assert state.yaw == 0
|
||||
assert state.yaw_frame_id == 'map'
|
||||
|
||||
# test set_position
|
||||
res = set_position(x=nan, y=nan, z=13, yaw=nan, frame_id='test2')
|
||||
assert res.success == True
|
||||
state = get_state()
|
||||
assert state.mode == State.MODE_POSITION
|
||||
assert state.yaw_mode == State.YAW_MODE_YAW
|
||||
assert state.x == 5
|
||||
assert state.y == 6
|
||||
assert state.z == 13
|
||||
assert state.yaw == 0
|
||||
assert state.xy_frame_id == 'map'
|
||||
assert state.z_frame_id == 'test2'
|
||||
assert state.yaw_frame_id == 'map'
|
||||
|
||||
# set_altitude should not change the mode
|
||||
res = set_altitude(z=3, frame_id='test')
|
||||
assert res.success == True
|
||||
state = get_state()
|
||||
assert state.mode == State.MODE_POSITION
|
||||
assert state.yaw_mode == State.YAW_MODE_YAW
|
||||
assert state.x == 5
|
||||
assert state.y == 6
|
||||
assert state.z == 3
|
||||
assert state.yaw == 0
|
||||
assert state.xy_frame_id == 'map'
|
||||
assert state.z_frame_id == 'test'
|
||||
assert state.yaw_frame_id == 'map'
|
||||
|
||||
# set_yaw should not change the main mode
|
||||
res = set_yaw(yaw=1, frame_id='test2')
|
||||
assert res.success == True
|
||||
state = get_state()
|
||||
assert state.mode == State.MODE_POSITION
|
||||
assert state.yaw_mode == State.YAW_MODE_YAW
|
||||
assert state.x == 5
|
||||
assert state.y == 6
|
||||
assert state.z == 3
|
||||
assert state.yaw == 1
|
||||
assert state.xy_frame_id == 'map'
|
||||
assert state.z_frame_id == 'test'
|
||||
assert state.yaw_frame_id == 'test2'
|
||||
|
||||
# test set_velocity
|
||||
res = set_velocity(vx=1, frame_id='body')
|
||||
state = get_state()
|
||||
assert state.mode == State.MODE_VELOCITY
|
||||
assert state.yaw_mode == State.YAW_MODE_YAW
|
||||
assert state.vx == 1
|
||||
assert state.vy == 0
|
||||
assert state.vz == 0
|
||||
assert state.yaw == 0
|
||||
assert state.xy_frame_id == 'map'
|
||||
assert state.z_frame_id == 'map'
|
||||
assert state.yaw_frame_id == 'map'
|
||||
|
||||
# set_altitude should not work in velocity mode
|
||||
res = set_altitude(z=3, frame_id='test')
|
||||
assert res.success == False
|
||||
assert res.message.startswith('Altitude cannot be set in')
|
||||
|
||||
# test set_attitude
|
||||
res = set_attitude(roll=0.1, pitch=0.2, yaw=0.3, thrust=0.5)
|
||||
assert res.success == True
|
||||
state = get_state()
|
||||
assert state.mode == State.MODE_ATTITUDE
|
||||
assert state.yaw_mode == State.YAW_MODE_YAW
|
||||
assert state.roll == approx(0.1)
|
||||
assert state.pitch == approx(0.2)
|
||||
assert state.yaw == approx(0.3)
|
||||
assert state.thrust == approx(0.5)
|
||||
assert state.yaw_frame_id == 'map'
|
||||
msg = rospy.wait_for_message('/mavros/setpoint_attitude/attitude', PoseStamped, timeout=3)
|
||||
# Tait-Bryan ZYX angle (rzyx) converted to quaternion
|
||||
assert msg.pose.orientation.x == approx(0.0342708)
|
||||
assert msg.pose.orientation.y == approx(0.10602051)
|
||||
assert msg.pose.orientation.z == approx(0.14357218)
|
||||
assert msg.pose.orientation.w == approx(0.98334744)
|
||||
msg = rospy.wait_for_message('/mavros/setpoint_attitude/thrust', mavros_msgs.msg.Thrust, timeout=3)
|
||||
assert msg.thrust == approx(0.5)
|
||||
|
||||
# set_yaw should work in attitude mode
|
||||
res = set_yaw(yaw=0.7, frame_id='test2')
|
||||
assert res.success == True
|
||||
state = get_state()
|
||||
assert state.mode == State.MODE_ATTITUDE
|
||||
assert state.yaw_mode == State.YAW_MODE_YAW
|
||||
assert state.roll == approx(0.1)
|
||||
assert state.pitch == approx(0.2)
|
||||
assert state.yaw == approx(0.7)
|
||||
assert state.thrust == approx(0.5)
|
||||
assert state.yaw_frame_id == 'test2'
|
||||
|
||||
# set_yaw_rate should not work in attitude mode
|
||||
res = set_yaw_rate(yaw_rate=0.3)
|
||||
assert res.success == False
|
||||
assert res.message.startswith('Yaw rate cannot be set in')
|
||||
|
||||
# test set_rates
|
||||
res = set_rates(roll_rate=nan, pitch_rate=nan, yaw_rate=0.3, thrust=0.6)
|
||||
assert res.success == True
|
||||
state = get_state()
|
||||
assert state.mode == State.MODE_RATES
|
||||
assert state.yaw_mode == State.YAW_MODE_YAW_RATE
|
||||
assert state.roll_rate == approx(0)
|
||||
assert state.pitch_rate == approx(0)
|
||||
assert state.yaw_rate == approx(0.3)
|
||||
assert state.thrust == approx(0.6)
|
||||
msg = rospy.wait_for_message('/mavros/setpoint_raw/attitude', mavros_msgs.msg.AttitudeTarget, timeout=3)
|
||||
assert msg.thrust == approx(0.6)
|
||||
|
||||
res = set_rates(roll_rate=0.3, pitch_rate=0.2, yaw_rate=0.1, thrust=0.4)
|
||||
assert res.success == True
|
||||
state = get_state()
|
||||
assert state.mode == State.MODE_RATES
|
||||
assert state.yaw_mode == State.YAW_MODE_YAW_RATE
|
||||
assert state.roll_rate == approx(0.3)
|
||||
assert state.pitch_rate == approx(0.2)
|
||||
assert state.yaw_rate == approx(0.1)
|
||||
assert state.thrust == approx(0.4)
|
||||
|
||||
res = set_rates(roll_rate=nan, pitch_rate=nan, yaw_rate=nan, thrust=0.3)
|
||||
assert res.success == True
|
||||
state = get_state()
|
||||
assert state.mode == State.MODE_RATES
|
||||
assert state.yaw_mode == State.YAW_MODE_YAW_RATE
|
||||
assert state.roll_rate == approx(0.3)
|
||||
assert state.pitch_rate == approx(0.2)
|
||||
assert state.yaw_rate == approx(0.1)
|
||||
assert state.thrust == approx(0.3)
|
||||
msg = rospy.wait_for_message('/mavros/setpoint_raw/attitude', mavros_msgs.msg.AttitudeTarget, timeout=3)
|
||||
assert msg.type_mask == mavros_msgs.msg.AttitudeTarget.IGNORE_ATTITUDE
|
||||
assert msg.body_rate.x == approx(0.3)
|
||||
assert msg.body_rate.y == approx(0.2)
|
||||
assert msg.body_rate.z == approx(0.1)
|
||||
|
||||
# set_yaw_rate should work in rates mode
|
||||
res = set_yaw_rate(yaw_rate=0.4)
|
||||
assert res.success == True
|
||||
state = get_state()
|
||||
assert state.mode == State.MODE_RATES
|
||||
assert state.yaw_mode == State.YAW_MODE_YAW_RATE
|
||||
assert state.roll_rate == approx(0.3)
|
||||
assert state.pitch_rate == approx(0.2)
|
||||
assert state.yaw_rate == approx(0.4)
|
||||
assert state.thrust == approx(0.3)
|
||||
|
||||
res = set_rates(roll_rate=inf)
|
||||
assert res.success == False
|
||||
assert res.message == 'roll_rate argument cannot be Inf'
|
||||
|
||||
# test land service
|
||||
res = land()
|
||||
assert res.success == True
|
||||
assert state_msg.mode == 'AUTO.LAND' # check that the mode was set correctly
|
||||
@@ -1,10 +0,0 @@
|
||||
<launch>
|
||||
<node name="simple_offboard" pkg="clover" type="simple_offboard" required="true" output="screen"/>
|
||||
|
||||
<node pkg="tf2_ros" type="static_transform_publisher" name="test_frame" args="10 20 30 0 0 0 map test"/>
|
||||
|
||||
<node pkg="tf2_ros" type="static_transform_publisher" name="test2_frame" args="100 200 300 0 0 0 map test2"/>
|
||||
|
||||
<param name="test_module" value="$(find clover)/test/offboard.py"/>
|
||||
<test test-name="offboard_test" pkg="ros_pytest" type="ros_pytest_runner"/>
|
||||
</launch>
|
||||
@@ -12,6 +12,4 @@ SUBSYSTEM=="tty", ATTRS{idVendor}=="26ac", ATTRS{idProduct}=="0016", ATTRS{produ
|
||||
SUBSYSTEM=="tty", ATTRS{idVendor}=="26ac", ATTRS{idProduct}=="0013", ATTRS{product}=="PX4 FMU v4.x PRO", SYMLINK+="px4fmu"
|
||||
# Omnibus
|
||||
SUBSYSTEM=="tty", ATTRS{idVendor}=="26ac", ATTRS{idProduct}=="0001", ATTRS{product}=="PX4 OmnibusF4SD", SYMLINK+="px4fmu"
|
||||
# CUAV X7 Pro
|
||||
SUBSYSTEM=="tty", ATTRS{idVendor}=="3163", ATTRS{idProduct}=="004c", ATTRS{product}=="PX4 CUAV X7Pro", SYMLINK+="px4fmu"
|
||||
|
||||
|
||||
@@ -1,23 +0,0 @@
|
||||
<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>
|
||||
@@ -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="console.html">Clover console</a> (<code>/var/log/clover.log</code>)</li>
|
||||
<li><a href="clover.log">Clover console</a> (<code>/var/log/clover.log</code>)</li>
|
||||
</ul>
|
||||
|
||||
<div class="version"></div>
|
||||
|
||||
@@ -1,16 +1,13 @@
|
||||
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');
|
||||
document.body.classList.remove('closed');
|
||||
init();
|
||||
});
|
||||
|
||||
ros.on('close', function () {
|
||||
document.body.classList.remove('connected');
|
||||
document.body.classList.add('closed');
|
||||
setTimeout(function() {
|
||||
// reconnect
|
||||
ros.connect(url);
|
||||
@@ -40,32 +37,42 @@ function viewTopicsList() {
|
||||
let rosdistro;
|
||||
|
||||
function viewTopic(topic) {
|
||||
let counter = 0;
|
||||
let index = '<a href=topics.html>Topics</a>';
|
||||
title.innerHTML = `${index}: ${topic}`;
|
||||
title.innerHTML = topic;
|
||||
topicMessage.style.display = 'block';
|
||||
|
||||
ros.getTopicType(topic, function(typeStr) {
|
||||
const [pack, type] = typeStr.split('/');
|
||||
let href = `https://docs.ros.org/en/${rosdistro}/api/${pack}/html/msg/${type}.html`;
|
||||
title.innerHTML = `${index}: ${topic} <a id="topic-type" href=${href} target="_blank">${typeStr}</a>`;
|
||||
title.innerHTML = `${topic} <a id="topic-type" href=${href} target="_blank">${typeStr}</a>`;
|
||||
});
|
||||
|
||||
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;
|
||||
}
|
||||
}
|
||||
function viewParameters() {
|
||||
title.innerHTML = 'Parameters';
|
||||
topicMessage.style.display = 'block';
|
||||
|
||||
let txt = `<div class=counter>${counter} received</div>${yamlStringify(msg)}`; // JSON.stringify(msg, null, 4);
|
||||
topicMessage.innerHTML = txt;
|
||||
let names = ['aruco_detect', 'main_camera', 'main_camera_nodelet_manager', 'mavros', 'optical_flow',
|
||||
'rangefinder', 'rosapi', 'rosbridge_websocket', 'rosdistro', 'roslaunch', 'rosversion',
|
||||
'roswww_static', 'run_id', 'simple_offboard', 'visualization', 'web_video_server'];
|
||||
let params = {};
|
||||
// ros.getParams(function(params) {
|
||||
|
||||
Promise.all(names.map(function(name) {
|
||||
return new Promise(function(resolve, reject) {
|
||||
new ROSLIB.Param({ ros, name }).get(function(value) {
|
||||
params[name] = value;
|
||||
resolve();
|
||||
})
|
||||
});
|
||||
})).then(function() {
|
||||
console.log(params);
|
||||
topicMessage.innerHTML = yamlStringify(params);
|
||||
});
|
||||
}
|
||||
|
||||
@@ -75,6 +82,11 @@ topicMessage.addEventListener('mousedown', function() { mouseDown = true; });
|
||||
topicMessage.addEventListener('mouseup', function() { mouseDown = false; });
|
||||
|
||||
function init() {
|
||||
const params = Object.fromEntries(new URLSearchParams(window.location.search).entries());
|
||||
|
||||
viewParameters();
|
||||
return;
|
||||
|
||||
if (!params.topic) {
|
||||
viewTopicsList();
|
||||
} else {
|
||||
@@ -84,3 +96,5 @@ function init() {
|
||||
});
|
||||
}
|
||||
}
|
||||
|
||||
window.ros = ros;
|
||||
|
||||
@@ -15,15 +15,13 @@
|
||||
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); }
|
||||
</style>
|
||||
</head>
|
||||
<body>
|
||||
<h1> </h1>
|
||||
<ul id="topics"></ul>
|
||||
<code id="topic-message">No messages received</code>
|
||||
<code id="topic-message" title="Hold mouse button to pause">No messages received</code>
|
||||
</body>
|
||||
</html>
|
||||
|
||||
@@ -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/noetic/api/std_srvs/html/srv/Trigger.html)) – terminate the running program.
|
||||
* `~stop` ([*std_srvs/Trigger*](http://docs.ros.org/melodic/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,12 +45,11 @@ http://<hostname>/clover_blocks/?navigate_tolerance=0.5&sleep_time=0.1
|
||||
|
||||
#### Published
|
||||
|
||||
* `~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).
|
||||
* `~print` ([*std_msgs/String*](http://docs.ros.org/noetic/api/std_msgs/html/msg/String.html)) – user program output messages (published in *print* blocks).
|
||||
* `~error` ([*std_msgs/String*](http://docs.ros.org/noetic/api/std_msgs/html/msg/String.html)) – user program errors and exceptions.
|
||||
* `~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.
|
||||
* `~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/noetic/api/std_msgs/html/msg/String.html)) – user input response.
|
||||
* `~prompt/<request_id>` ([*std_msgs/String*](http://docs.ros.org/melodic/api/std_msgs/html/msg/String.html)) – user input response.
|
||||
|
||||
@@ -1,7 +1,7 @@
|
||||
<?xml version="1.0"?>
|
||||
<package format="2">
|
||||
<name>clover_blocks</name>
|
||||
<version>0.24.0</version>
|
||||
<version>0.21.1</version>
|
||||
<description>Blockly programming support for Clover</description>
|
||||
<maintainer email="okalachev@gmail.com">Oleg Kalachev</maintainer>
|
||||
<license>MIT</license>
|
||||
|
||||
@@ -15,7 +15,6 @@ const COLOR_GPIO = 200;
|
||||
const DOCS_URL = 'https://clover.coex.tech/en/blocks.html';
|
||||
|
||||
var frameIds = [["body", "BODY"], ["markers map", "ARUCO_MAP"], ["marker", "ARUCO"], ["last navigate target", "NAVIGATE_TARGET"], ["map", "MAP"]];
|
||||
var frameIdsWithTerrain = frameIds.concat([["terrain", "TERRAIN"]]);
|
||||
|
||||
function considerFrameId(e) {
|
||||
if (!(e instanceof Blockly.Events.Change || e instanceof Blockly.Events.Create)) return;
|
||||
@@ -23,7 +22,7 @@ function considerFrameId(e) {
|
||||
var frameId = this.getFieldValue('FRAME_ID');
|
||||
// set appropriate coordinates labels
|
||||
if (this.getInput('X')) { // block has x-y-z fields
|
||||
if (frameId == 'BODY' || frameId == 'NAVIGATE_TARGET' || frameId == 'BASE_LINK' || frameId == 'TERRAIN') {
|
||||
if (frameId == 'BODY' || frameId == 'NAVIGATE_TARGET' || frameId == 'BASE_LINK') {
|
||||
this.getInput('X').fieldRow[0].setValue('forward');
|
||||
this.getInput('Y').fieldRow[0].setValue('left');
|
||||
this.getInput('Z').fieldRow[0].setValue('up');
|
||||
@@ -60,8 +59,8 @@ function updateSetpointBlock(e) {
|
||||
this.getInput('VY').setVisible(velocity);
|
||||
this.getInput('VZ').setVisible(velocity);
|
||||
this.getInput('YAW').setVisible(attitude);
|
||||
this.getInput('ROLL').setVisible(attitude);
|
||||
this.getInput('PITCH').setVisible(attitude);
|
||||
this.getInput('ROLL').setVisible(attitude);
|
||||
this.getInput('THRUST').setVisible(attitude);
|
||||
this.getInput('RELATIVE_TO').setVisible(type != 'RATES');
|
||||
|
||||
@@ -74,7 +73,7 @@ function updateSetpointBlock(e) {
|
||||
|
||||
Blockly.Blocks['navigate'] = {
|
||||
init: function () {
|
||||
let navFrameId = frameIdsWithTerrain.slice();
|
||||
let navFrameId = frameIds.slice();
|
||||
navFrameId.push(['global', 'GLOBAL_LOCAL'])
|
||||
navFrameId.push(['global, WGS 84 alt.', 'GLOBAL'])
|
||||
this.appendDummyInput()
|
||||
@@ -164,14 +163,14 @@ Blockly.Blocks['setpoint'] = {
|
||||
this.appendValueInput("VZ")
|
||||
.setCheck("Number")
|
||||
.appendField("vz");
|
||||
this.appendValueInput("ROLL")
|
||||
.setCheck("Number")
|
||||
.appendField("roll")
|
||||
.setVisible(false);
|
||||
this.appendValueInput("PITCH")
|
||||
.setCheck("Number")
|
||||
.appendField("pitch")
|
||||
.setVisible(false);
|
||||
this.appendValueInput("ROLL")
|
||||
.setCheck("Number")
|
||||
.appendField("roll")
|
||||
.setVisible(false);
|
||||
this.appendValueInput("YAW")
|
||||
.setCheck("Number")
|
||||
.appendField("yaw")
|
||||
@@ -214,7 +213,7 @@ Blockly.Blocks['get_position'] = {
|
||||
.appendField("current")
|
||||
.appendField(new Blockly.FieldDropdown([["x", "X"], ["y", "Y"], ["z", "Z"], ["vx", "VX"], ["vy", "VY"], ["vz", "VZ"]]), "FIELD")
|
||||
.appendField("relative to")
|
||||
.appendField(new Blockly.FieldDropdown(frameIdsWithTerrain), "FRAME_ID");
|
||||
.appendField(new Blockly.FieldDropdown(frameIds), "FRAME_ID");
|
||||
this.appendValueInput("ID")
|
||||
.setCheck("Number")
|
||||
.appendField("with ID")
|
||||
@@ -248,7 +247,7 @@ Blockly.Blocks['get_attitude'] = {
|
||||
init: function () {
|
||||
this.appendDummyInput()
|
||||
.appendField("current")
|
||||
.appendField(new Blockly.FieldDropdown([["roll", "ROLL"], ["pitch", "PITCH"], ["roll rate", "ROLL_RATE"], ["pitch rate", "PITCH_RATE"], ["yaw rate", "YAW_RATE"]]), "FIELD");
|
||||
.appendField(new Blockly.FieldDropdown([["pitch", "PITCH"], ["roll", "ROLL"], ["pitch rate", "PITCH_RATE"], ["roll rate", "ROLL_RATE"], ["yaw rate", "YAW_RATE"]]), "FIELD");
|
||||
this.setOutput(true, "Number");
|
||||
this.setColour(COLOR_STATE);
|
||||
this.setTooltip("Returns current orientation or angle rates in degree or degree per second (not radian).");
|
||||
@@ -269,19 +268,6 @@ Blockly.Blocks['voltage'] = {
|
||||
}
|
||||
};
|
||||
|
||||
Blockly.Blocks['get_rc'] = {
|
||||
init: function () {
|
||||
this.appendDummyInput()
|
||||
.appendField("RC channel")
|
||||
this.appendValueInput("CHANNEL")
|
||||
.setCheck("Number");
|
||||
this.setInputsInline(true);
|
||||
this.setOutput(true, "Number");
|
||||
this.setColour(COLOR_STATE);
|
||||
this.setTooltip("Returns current RC channel value.");
|
||||
this.setHelpUrl(DOCS_URL + '#' + this.type);
|
||||
}
|
||||
}
|
||||
|
||||
Blockly.Blocks['armed'] = {
|
||||
init: function () {
|
||||
@@ -523,7 +509,7 @@ Blockly.Blocks['distance'] = {
|
||||
.appendField("z");
|
||||
this.appendDummyInput()
|
||||
.appendField("relative to")
|
||||
.appendField(new Blockly.FieldDropdown([["markers map", "ARUCO_MAP"], ["marker", "ARUCO"], ["last navigate target", "NAVIGATE_TARGET"], ["terrain", "TERRAIN"]]), "FRAME_ID");
|
||||
.appendField(new Blockly.FieldDropdown([["markers map", "ARUCO_MAP"], ["marker", "ARUCO"], ["last navigate target", "NAVIGATE_TARGET"]]), "FRAME_ID");
|
||||
this.appendValueInput("ID")
|
||||
.setCheck("Number")
|
||||
.appendField("with ID")
|
||||
|
||||
@@ -69,8 +69,8 @@
|
||||
<value name="VX"><shadow type="math_number"><field name="NUM">0</field></shadow></value>
|
||||
<value name="VY"><shadow type="math_number"><field name="NUM">0</field></shadow></value>
|
||||
<value name="VZ"><shadow type="math_number"><field name="NUM">0</field></shadow></value>
|
||||
<value name="ROLL"><shadow type="math_number"><field name="NUM">0</field></shadow></value>
|
||||
<value name="PITCH"><shadow type="math_number"><field name="NUM">0</field></shadow></value>
|
||||
<value name="ROLL"><shadow type="math_number"><field name="NUM">0</field></shadow></value>
|
||||
<value name="YAW"><shadow type="math_number"><field name="NUM">0</field></shadow></value>
|
||||
<value name="THRUST"><shadow type="math_number"><field name="NUM">0.5</field></shadow></value>
|
||||
<value name="ID"><shadow type="math_number"><field name="NUM">0</field></shadow></value>
|
||||
@@ -100,9 +100,6 @@
|
||||
<block type="mode"></block>
|
||||
<block type="armed"></block>
|
||||
<block type="voltage"></block>
|
||||
<block type="get_rc">
|
||||
<value name="CHANNEL"><shadow type="math_number"><field name="NUM">0</field></shadow></value>
|
||||
</block>
|
||||
</category>
|
||||
<category name="LED" colour="#02d754">
|
||||
<block type="set_effect">
|
||||
|
||||
@@ -81,10 +81,7 @@ function generateROSDefinitions() {
|
||||
code += `get_telemetry = rospy.ServiceProxy('get_telemetry', srv.GetTelemetry)\n`;
|
||||
code += `navigate = rospy.ServiceProxy('navigate', srv.Navigate)\n`;
|
||||
if (rosDefinitions.navigateGlobal) {
|
||||
code += `navigate_global = rospy.ServiceProxy('navigate_global', srv.NavigateGlobal)\n`;
|
||||
}
|
||||
if (rosDefinitions.setYaw) {
|
||||
code += `set_yaw = rospy.ServiceProxy('set_yaw', srv.SetYaw)\n`;
|
||||
code += `navigate_global = rospy.ServiceProxy('navigate_global', srv.NavigateGlobal)\n`;
|
||||
}
|
||||
if (rosDefinitions.setVelocity) {
|
||||
code += `set_velocity = rospy.ServiceProxy('set_velocity', srv.SetVelocity)\n`;
|
||||
@@ -279,11 +276,10 @@ Blockly.Python.angle = function(block) {
|
||||
}
|
||||
|
||||
Blockly.Python.set_yaw = function(block) {
|
||||
rosDefinitions.setYaw = true;
|
||||
simpleOffboard();
|
||||
let yaw = Blockly.Python.valueToCode(block, 'YAW', Blockly.Python.ORDER_NONE);
|
||||
let frameId = buildFrameId(block);
|
||||
let code = `set_yaw(yaw=${yaw}, frame_id=${frameId})\n`;
|
||||
let code = `navigate(x=float('nan'), y=float('nan'), z=float('nan'), yaw=${yaw}, frame_id=${frameId})\n`;
|
||||
if (block.getFieldValue('WAIT') == 'TRUE') {
|
||||
rosDefinitions.waitYaw = true;
|
||||
simpleOffboard();
|
||||
@@ -332,11 +328,11 @@ Blockly.Python.setpoint = function(block) {
|
||||
} else if (type == 'ATTITUDE') {
|
||||
rosDefinitions.setAttitude = true;
|
||||
simpleOffboard();
|
||||
return `set_attitude(roll=${roll}, pitch=${pitch}, yaw=${yaw}, thrust=${thrust}, frame_id=${frameId})\n`;
|
||||
return `set_attitude(pitch=${pitch}, roll=${roll}, yaw=${yaw}, thrust=${thrust}, frame_id=${frameId})\n`;
|
||||
} else if (type == 'RATES') {
|
||||
rosDefinitions.setRates = true;
|
||||
simpleOffboard();
|
||||
return `set_rates(roll_rate=${roll}, pitch_rate=${pitch}, yaw_rate=${yaw}, thrust=${thrust})\n`;
|
||||
return `set_rates(pitch_rate=${pitch}, roll_rate=${roll}, yaw_rate=${yaw}, thrust=${thrust})\n`;
|
||||
}
|
||||
}
|
||||
|
||||
@@ -402,12 +398,6 @@ Blockly.Python.voltage = function(block) {
|
||||
return [code, Blockly.Python.ORDER_FUNCTION_CALL];
|
||||
}
|
||||
|
||||
Blockly.Python.get_rc = function(block) {
|
||||
Blockly.Python.definitions_['import_rcin'] = 'from mavros_msgs.msg import RCIn';
|
||||
var channel = Blockly.Python.valueToCode(block, 'CHANNEL', Blockly.Python.ORDER_NONE);
|
||||
return [`rospy.wait_for_message('mavros/rc/in', RCIn).channels[${channel}]`, Blockly.Python.ORDER_FUNCTION_CALL]
|
||||
}
|
||||
|
||||
function parseColor(color) {
|
||||
return {
|
||||
r: parseInt(color.substr(2, 2), 16),
|
||||
@@ -474,7 +464,7 @@ Blockly.Python.led_count = function(block) {
|
||||
|
||||
function pigpio() {
|
||||
Blockly.Python.definitions_['import_pigpio'] = 'import pigpio';
|
||||
Blockly.Python.definitions_['init_pigpio'] = 'pi = pigpio.pi()\nif not pi.connected: raise Exception(\'Cannot connect to pigpiod\')';
|
||||
Blockly.Python.definitions_['init_pigpio'] = 'pi = pigpio.pi()';
|
||||
}
|
||||
|
||||
const GPIO_READ = `\ndef gpio_read(pin):
|
||||
|
||||
@@ -1,6 +1,6 @@
|
||||
<package format="2">
|
||||
<name>clover_description</name>
|
||||
<version>0.24.0</version>
|
||||
<version>0.21.1</version>
|
||||
<description>The clover_description package provides URDF models of the Clover series of quadcopters.</description>
|
||||
|
||||
<maintainer email="sfalexrog@gmail.com">Alexey Rogachevskiy</maintainer>
|
||||
|
||||
@@ -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="DarkGrey" />
|
||||
<xacro:property name="color" value="$(arg visual_material)" />
|
||||
|
||||
<!-- Property Blocks -->
|
||||
<!-- Clover body inertia -->
|
||||
|
||||
@@ -64,12 +64,6 @@
|
||||
<!-- <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>
|
||||
|
||||
@@ -2,7 +2,7 @@
|
||||
<robot name="rpi_camera" xmlns:xacro="http://ros.org/wiki/xacro">
|
||||
<xacro:include filename="../camera_sensor.urdf.xacro"/>
|
||||
|
||||
<xacro:macro name="distance_sensor" params="name:=lidar_vl53l1x parent x:=0 y:=0 z:=0 roll:=0 pitch:=0 yaw:=0 range_min:=0.01 range_max:=4.0 resolution:=0.01 rate:=10 fov:=0.471239">
|
||||
<xacro:macro name="distance_sensor" params="name:=lidar_vl53l1x parent x:=0 y:=0 z:=0 roll:=0 pitch:=0 yaw:=0 range_min:=0.01 range_max:=4.0 resolution:=0.01 rate:=10">
|
||||
<joint name="${name}_joint" type="fixed">
|
||||
<origin xyz="${x} ${y} ${z}"
|
||||
rpy="${roll} ${pitch} ${yaw}"/>
|
||||
@@ -58,7 +58,7 @@
|
||||
<topicName>/rangefinder/range</topicName>
|
||||
<frameName>rangefinder</frameName>
|
||||
<radiation>infrared</radiation>
|
||||
<fov>${fov}</fov>
|
||||
<fov>0.01</fov>
|
||||
<gaussianNoise>0.001</gaussianNoise>
|
||||
<updateRate>${rate}</updateRate>
|
||||
<min_distance>${range_min}</min_distance>
|
||||
|
||||
@@ -37,14 +37,6 @@ 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}
|
||||
|
||||
@@ -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.
|
||||
* `vehicle` (*string*, default: *clover*) - PX4 vehicle name. Depending on this parameter, different PX4 presets will be loaded. **Note**: The default value, *clover*, requires you to use [Clover-specific PX4 branch](https://github.com/CopterExpress/Firmware/tree/v1.10.1-clever);
|
||||
* `main_camera` (*boolean*, default: *true*) - controls whether the drone will have a vision position estimation camera;
|
||||
* `rangefinder` (*boolean*, default: *true*) - controls whether the drone will have a laser rangefinder;
|
||||
* `led` (*boolean*, default: *true*) - controls whether the drone will have a programmable LED strip;
|
||||
|
||||
@@ -7,31 +7,30 @@
|
||||
|
||||
. ${R}etc/init.d-posix/airframes/10016_iris # base on iris
|
||||
|
||||
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 ATT_W_EXT_HDG 0.5
|
||||
param set ATT_EXT_HDG_M 1
|
||||
|
||||
param set-default COM_DISARM_LAND 1.0
|
||||
param set-default COM_RCL_EXCEPT 4 # enable offboard flights without rc
|
||||
param set COM_DISARM_LAND 1.0
|
||||
|
||||
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 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 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 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 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 3 # 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
|
||||
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
|
||||
|
||||
@@ -4,7 +4,6 @@
|
||||
<arg name="est" default="ekf2"/> <!-- PX4 estimator: lpe, ekf2 -->
|
||||
<arg name="vehicle" default="clover"/> <!-- PX4 vehicle configuration: clover, clover_vpe -->
|
||||
<arg name="main_camera" default="true"/> <!-- Simulated vision position estimation camera (optical flow, ArUco) -->
|
||||
<arg name="maintain_camera_rate" default="false"/> <!-- Slow simulation down to maintain camera rate -->
|
||||
<arg name="rangefinder" default="true"/> <!-- Simulated downward-facing rangefinder, vl53l1x-like -->
|
||||
<arg name="led" default="true"/> <!-- Simulated LED strip, ws281x-like -->
|
||||
<arg name="gps" default="false"/> <!--Simulated GPS module -->
|
||||
@@ -21,7 +20,7 @@
|
||||
</include>
|
||||
|
||||
<!-- PX4 instance -->
|
||||
<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')">
|
||||
<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')">
|
||||
<env name="PX4_SIM_MODEL" value="$(arg vehicle)"/>
|
||||
<env name="PX4_ESTIMATOR" value="$(arg est)"/>
|
||||
</node>
|
||||
@@ -29,14 +28,13 @@
|
||||
<!-- Clover model -->
|
||||
<include file="$(find clover_description)/launch/spawn_drone.launch" if="$(eval type == 'gazebo')">
|
||||
<arg name="main_camera" value="$(arg main_camera)"/>
|
||||
<arg name="maintain_camera_rate" value="$(arg maintain_camera_rate)"/>
|
||||
<arg name="rangefinder" value="$(arg rangefinder)"/>
|
||||
<arg name="led" value="$(arg led)"/>
|
||||
<arg name="gps" value="$(arg gps)"/>
|
||||
<arg name="use_clover_physics" value="$(arg use_clover_physics)"/>
|
||||
</include>
|
||||
|
||||
<node name="jmavsim" pkg="px4" required="true" type="jmavsim_run.sh" output="screen" if="$(eval type == 'jmavsim')">
|
||||
<node name="jmavsim" pkg="px4" type="jmavsim_run.sh" output="screen" if="$(eval type == 'jmavsim')">
|
||||
<env name="HEADLESS" value="1" if="$(eval not gui)"/>
|
||||
</node>
|
||||
|
||||
|
||||
@@ -1,24 +0,0 @@
|
||||
<?xml version="1.0"?>
|
||||
<sdf version="1.5">
|
||||
<model name="aruco_100">
|
||||
<static>true</static>
|
||||
<link name="marker_100_link">
|
||||
<pose>0 0 1e-3 0 0 1.5707963267948966</pose>
|
||||
<visual name="visual_marker_100">
|
||||
<cast_shadows>false</cast_shadows>
|
||||
<geometry>
|
||||
<box>
|
||||
<size>0.22 0.22 1e-3</size>
|
||||
</box>
|
||||
</geometry>
|
||||
<material>
|
||||
<script>
|
||||
<uri>model://aruco_100/materials/scripts</uri>
|
||||
<uri>model://aruco_100/materials/textures</uri>
|
||||
<name>aruco/marker_100</name>
|
||||
</script>
|
||||
</material>
|
||||
</visual>
|
||||
</link>
|
||||
</model>
|
||||
</sdf>
|
||||
@@ -1,15 +0,0 @@
|
||||
material aruco/marker_100
|
||||
{
|
||||
technique
|
||||
{
|
||||
pass
|
||||
{
|
||||
texture_unit
|
||||
{
|
||||
texture aruco_marker_100.png
|
||||
filtering none
|
||||
scale 1.0 1.0
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
Binary file not shown.
|
Before Width: | Height: | Size: 94 B |
@@ -1,13 +0,0 @@
|
||||
<?xml version="1.0"?>
|
||||
<model>
|
||||
<name>ArUco Marker 100</name>
|
||||
<version>1.0</version>
|
||||
<sdf version="1.5">marker_100.sdf</sdf>
|
||||
<author>
|
||||
<name>Marker Generator script</name>
|
||||
<email>marker@generator.sh</email>
|
||||
</author>
|
||||
<description>
|
||||
ArUco marker #100
|
||||
</description>
|
||||
</model>
|
||||
@@ -1,24 +0,0 @@
|
||||
<?xml version="1.0"?>
|
||||
<sdf version="1.5">
|
||||
<model name="aruco_101">
|
||||
<static>true</static>
|
||||
<link name="marker_101_link">
|
||||
<pose>0 0 1e-3 0 0 1.5707963267948966</pose>
|
||||
<visual name="visual_marker_101">
|
||||
<cast_shadows>false</cast_shadows>
|
||||
<geometry>
|
||||
<box>
|
||||
<size>0.44 0.44 1e-3</size>
|
||||
</box>
|
||||
</geometry>
|
||||
<material>
|
||||
<script>
|
||||
<uri>model://aruco_101/materials/scripts</uri>
|
||||
<uri>model://aruco_101/materials/textures</uri>
|
||||
<name>aruco/marker_101</name>
|
||||
</script>
|
||||
</material>
|
||||
</visual>
|
||||
</link>
|
||||
</model>
|
||||
</sdf>
|
||||
@@ -1,15 +0,0 @@
|
||||
material aruco/marker_101
|
||||
{
|
||||
technique
|
||||
{
|
||||
pass
|
||||
{
|
||||
texture_unit
|
||||
{
|
||||
texture aruco_marker_101.png
|
||||
filtering none
|
||||
scale 1.0 1.0
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
Binary file not shown.
|
Before Width: | Height: | Size: 94 B |
@@ -1,13 +0,0 @@
|
||||
<?xml version="1.0"?>
|
||||
<model>
|
||||
<name>ArUco Marker 101</name>
|
||||
<version>1.0</version>
|
||||
<sdf version="1.5">marker_101.sdf</sdf>
|
||||
<author>
|
||||
<name>Marker Generator script</name>
|
||||
<email>marker@generator.sh</email>
|
||||
</author>
|
||||
<description>
|
||||
ArUco marker #101
|
||||
</description>
|
||||
</model>
|
||||
@@ -1,24 +0,0 @@
|
||||
<?xml version="1.0"?>
|
||||
<sdf version="1.5">
|
||||
<model name="aruco_102">
|
||||
<static>true</static>
|
||||
<link name="marker_102_link">
|
||||
<pose>0 0 1e-3 0 0 1.5707963267948966</pose>
|
||||
<visual name="visual_marker_102">
|
||||
<cast_shadows>false</cast_shadows>
|
||||
<geometry>
|
||||
<box>
|
||||
<size>0.44 0.44 1e-3</size>
|
||||
</box>
|
||||
</geometry>
|
||||
<material>
|
||||
<script>
|
||||
<uri>model://aruco_102/materials/scripts</uri>
|
||||
<uri>model://aruco_102/materials/textures</uri>
|
||||
<name>aruco/marker_102</name>
|
||||
</script>
|
||||
</material>
|
||||
</visual>
|
||||
</link>
|
||||
</model>
|
||||
</sdf>
|
||||
@@ -1,15 +0,0 @@
|
||||
material aruco/marker_102
|
||||
{
|
||||
technique
|
||||
{
|
||||
pass
|
||||
{
|
||||
texture_unit
|
||||
{
|
||||
texture aruco_marker_102.png
|
||||
filtering none
|
||||
scale 1.0 1.0
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
Binary file not shown.
|
Before Width: | Height: | Size: 94 B |
Some files were not shown because too many files have changed in this diff Show More
Reference in New Issue
Block a user