mirror of
https://github.com/CopterExpress/clover.git
synced 2026-05-26 11:43:25 +00:00
Move ROS Noetic (#327)
* builder: Use 64-bit Raspberry Pi OS * travis: Use 64-bit builder * builder: Don't try to install Melodic packages on Noetic * clover: Use package version 3, update dependencies * travis: Enable Noetic build * standalone_install: Auto-select Python, ROS distro * builder: Use variable substitution for ROS_DISTRO * builder: Add Noetic package definitions * builder: Use variable substitution for validation * aruco_pose, clover: Allow compiling against OpenCV 3 and 4 * builder: Add proper Noetic repository * builder: Don't force Tornado version Assume rosbridge_suite depends on the right one. * builder: Install packages for Python 3 * builder/test: Use Python3 interpreter for ROS tests TODO (?): add tests for Python2? * builder: Use Python 3 syntax for Python 3 tests * builder: Install rpi_ws281x for Python3 * standalone_install: Use proper Python for pytest * builder: Install espeak for python3 * builder: Use proper path for roscore * builder: Install rosdep, etc. for python3 * builder: Run Clever/Clover test with Python3 * builder: Use Python3 for Clever compat layer * builder: Enable OpenCV 4.2 repository * builder: Force versions for ROS packages that use OpenCV Also, hold their versions so that they don't get updated for no reason. * aruco_pose/draw: Replace OpenCV projection code with a rewrite * builder: Don't try to install compressed_transport twice * clover: Fix importing urllib for Python3 * aruco_pose, clover: Expose Python scripts through CMake * clover/selfcheck: Be more python3-compatible This is basically commita01d199890from buster-python3, not sure if it aged well. * roswww_static: Add python script installation * clover_blocks: Use Python3 syntax for exec * aruco_pose: Remove unused code * Melodic => Noetic in some docs * docs: add 0.22 migration article * docs: remove unneeded comment * docs: python 3 updates * docs: python 3 update in auto_setup article * docs: add ROS Noetic transition note * aruco.launch: add placement, length and map arguments * genmap.py: add -o argument for output file name * docs: use -o argument of genmap.py * simple_offboard: correctly check manual control timeout, separate it from kill switch check * blocks: force led_leds index to int * docs: update and fix 0.22 migration articles * blocks: fix set_leds with color-typed argument * aruco_gen: Open file in binary mode for Python3 compatibility * clover: Use proper variable in aruco.launch * led: change default number of leds to 72 * aruco_pose: Make sure there are no undefined symbols Also, compile in apriltag_quad_thresh.cpp - it contains some of the functions referenced in aruco.cpp, which would otherwise be undefined. * aruco_pose: Make vendored library compatible with older OpenCVs * aruco_pose, clover: Reduce the amount of OpenCV libs requested * aruco_pose, clover: Move subscriptions to the end of init * aruco_pose: Don't expose vendored library symbols * aruco_pose: Simplify dynamic parameter callback setting * builder: Build with debug symbols * clover: Attempt to respawn dying nodelets * Change Raspberry Pi OS to latest armhf, use packages.coex.tech as a source * Add CRYPTOGRAPHY_DONT_BUILD_RUST=1 * Fix Node.js installation * image: use older CMake (3.13.4-1) Fixing https://travis-ci.org/github/CopterExpress/clover/jobs/764367665#L6984 * image: update Raspberry Pi OS to 2021-03-04 * image: bring back moving ld.so.preload out of the way while building * Fix pthreads ld error * Try to fix pthreads ld error * Another attempt to fix pthreads ld error * Yet another attempt to fix pthreads ld error * Try to fix * Be verbose * Temporarily disable rc and camera_markers building * Fix standalone-install * Revert "Temporarily disable rc and camera_markers building" This reverts commite119220e91. * Try to fix * Try to fix * Revert "image: use older CMake (3.13.4-1)" This reverts commitdf28da0060. * Revert "Revert "image: use older CMake (3.13.4-1)"" This reverts commita28c774e8f. * Verbosity * Debugging * More debugging * Display all CMake variables * Try to fix * Another try to fix * Revert "Another try to fix" This reverts commit5a4c3a0da7. * Another try to fix * And another * And yet another * Continue... * Cleanup * Sources lists cleanup * More cleanup * Restore .git directory in clover repo * Fix building documentation * Fix documentation building in image * Trigger build to update ws281x package * Test * Disable unneeded hack * Disable hack * image: add cmake-modules package * www: add viewing clover.err file from web interface * Remove hacks * Show nodelet version * docs: add packages article * image: add image-view package for recording video from topics * Minor fix * CI: add Docker authentication on image build * CI: fix Bash syntax * CI: fix authentication in Docker * CI: move Melodic build and editorconfig-lint to GitHub Actions (#331) * Create main.yml * Update main.yml * Disable native Melodic build in Travis * Run editorconfig-lint in Actions * Let wget be less verbose * Test * Test ok * Disable editorconfig-lint in Travis * docs: add links to hardware sources * CI: move image building to GitHub actions (#335) * Start working on building image in GitHub actions * Trigger GitHub on push to any branch * Fix TRAVIS_TAG * Add compress image step * Disable image build in Travis * Add upload image step * Fix compress image * Fix * Fix * Minor fix * Trigger build on tag * Show images sizes not in human format * Upload only built image * Make prerelease * Upload assets on release not on tags * readme: change build badge to GitHub Actions * readme: add support chat badge * CI: move documentation building to GitHub Actions (#337) * CI: change docs target branch to actions * CI: change docs target branch to master * CI: use gh-pages target branch for docs * CI: split up to several workflows * CI: remove .travis.yml * CI: change apt to apt-get * CI: push documentation site to the main repo * builder: less verbosity * CI: add new key for apt Fixing https://github.com/CopterExpress/clover/runs/2700356960#step:3:74 * Add Noetic building to CI * Add test for QR recognition * Fix * Move QR recognition test to a separate file * Fix QR recognition code for Python 3 * Import SetLEDs, LEDStateArray, LEDState in tests * Add more imports to tests (from documentation) * Fix permissions * Fix standalone-install for Python 2 * Fix QR recognition test * Don’t use ROS for QR recognition test * docs: remove non-working example * Make v4l2 device file an argument in main_camera.launch * Wait for v4l2 device before launching the camera driver * Use exec in waitfile * Transfer main camera nodelet manager to main_camera.launch * Update cv_camera version to 0.5.1 * docs: minor fix * Revert cv_camera to 0.5.0 * Update Raspberry Pi OS to 2021-05-07 * docs: add link to the last ROS Melodic version. Co-authored-by: Alexey Rogachevskiy <sfalexrog@gmail.com>
This commit is contained in:
Oleg Kalachev
7
.github/workflows/build.yml
vendored
7
.github/workflows/build.yml
vendored
@@ -14,3 +14,10 @@ jobs:
|
||||
- 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
|
||||
build-noetic:
|
||||
runs-on: ubuntu-latest
|
||||
steps:
|
||||
- uses: actions/checkout@v2
|
||||
- name: Native Noetic build
|
||||
run: |
|
||||
docker run --rm -v $(pwd):/root/catkin_ws/src/clover ros:noetic-ros-base /root/catkin_ws/src/clover/builder/standalone-install.sh
|
||||
|
||||
@@ -21,6 +21,7 @@
|
||||
"ROS",
|
||||
"ROS Kinetic",
|
||||
"ROS Melodic",
|
||||
"ROS Noetic",
|
||||
"OpenCV",
|
||||
"OpenVPN",
|
||||
"Gazebo",
|
||||
|
||||
@@ -26,7 +26,7 @@ Preconfigured image for Raspberry Pi with installed and configured software, rea
|
||||
Image features:
|
||||
|
||||
* Raspbian Buster
|
||||
* [ROS Melodic](http://wiki.ros.org/melodic)
|
||||
* [ROS Noetic](http://wiki.ros.org/noetic)
|
||||
* Configured networking
|
||||
* OpenCV
|
||||
* [`mavros`](http://wiki.ros.org/mavros)
|
||||
|
||||
@@ -22,13 +22,21 @@ find_package(catkin REQUIRED COMPONENTS
|
||||
dynamic_reconfigure
|
||||
)
|
||||
|
||||
find_package(OpenCV 3 REQUIRED COMPONENTS core imgproc calib3d)
|
||||
# Workaround for OpenCV 3/4 support
|
||||
set(_opencv_version 4)
|
||||
find_package(OpenCV ${_opencv_version} QUIET COMPONENTS core imgproc calib3d)
|
||||
if (NOT OpenCV_FOUND)
|
||||
message(STATUS "Did not find OpenCV 4, searching for OpenCV 3")
|
||||
set(_opencv_version 3)
|
||||
endif()
|
||||
|
||||
find_package(OpenCV ${_opencv_version} REQUIRED COMPONENTS core imgproc calib3d)
|
||||
if ("${OpenCV_VERSION_MINOR}" LESS "9")
|
||||
message(STATUS "OpenCV version too low, using vendored ArUco package")
|
||||
include(vendor/VendorOpenCV.cmake)
|
||||
else()
|
||||
message(STATUS "Using system OpenCV ArUco package")
|
||||
find_package(OpenCV 3 REQUIRED COMPONENTS aruco)
|
||||
find_package(OpenCV ${_opencv_version} REQUIRED COMPONENTS aruco)
|
||||
endif()
|
||||
message(STATUS "OpenCV include dirs: ${OpenCV_INCLUDE_DIRS}")
|
||||
message(STATUS "OpenCV libraries: ${OpenCV_LIBRARIES}")
|
||||
@@ -172,6 +180,13 @@ target_link_libraries(aruco_pose
|
||||
${OpenCV_LIBRARIES}
|
||||
)
|
||||
|
||||
# Prevent aruco_pose from having undefined symbols
|
||||
set_property(TARGET aruco_pose
|
||||
APPEND
|
||||
PROPERTY LINK_FLAGS
|
||||
-Wl,--no-undefined
|
||||
)
|
||||
|
||||
#############
|
||||
## Install ##
|
||||
#############
|
||||
@@ -207,6 +222,10 @@ target_link_libraries(aruco_pose
|
||||
# DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
|
||||
# )
|
||||
|
||||
catkin_install_python(PROGRAMS src/genmap.py
|
||||
DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
|
||||
)
|
||||
|
||||
#############
|
||||
## Testing ##
|
||||
#############
|
||||
|
||||
@@ -112,10 +112,7 @@ public:
|
||||
image_transport::ImageTransport it_priv(nh_priv_);
|
||||
|
||||
dyn_srv_ = std::make_shared<dynamic_reconfigure::Server<aruco_pose::DetectorConfig>>(nh_priv_);
|
||||
dynamic_reconfigure::Server<aruco_pose::DetectorConfig>::CallbackType cb;
|
||||
|
||||
cb = std::bind(&ArucoDetect::paramCallback, this, std::placeholders::_1, std::placeholders::_2);
|
||||
dyn_srv_->setCallback(cb);
|
||||
dyn_srv_->setCallback(std::bind(&ArucoDetect::paramCallback, this, std::placeholders::_1, std::placeholders::_2));
|
||||
|
||||
debug_pub_ = it_priv.advertise("debug", 1);
|
||||
markers_pub_ = nh_priv_.advertise<aruco_pose::MarkerArray>("markers", 1);
|
||||
|
||||
@@ -3,26 +3,11 @@
|
||||
|
||||
#include "draw.h"
|
||||
#include <math.h>
|
||||
#include <vector>
|
||||
|
||||
using namespace cv;
|
||||
using namespace cv::aruco;
|
||||
|
||||
static void _cvProjectPoints2( const CvMat* object_points, const CvMat* rotation_vector,
|
||||
const CvMat* translation_vector, const CvMat* camera_matrix,
|
||||
const CvMat* distortion_coeffs, CvMat* image_points,
|
||||
CvMat* dpdrot CV_DEFAULT(NULL), CvMat* dpdt CV_DEFAULT(NULL),
|
||||
CvMat* dpdf CV_DEFAULT(NULL), CvMat* dpdc CV_DEFAULT(NULL),
|
||||
CvMat* dpddist CV_DEFAULT(NULL),
|
||||
double aspect_ratio CV_DEFAULT(0));
|
||||
|
||||
static void _projectPoints( InputArray objectPoints,
|
||||
InputArray rvec, InputArray tvec,
|
||||
InputArray cameraMatrix, InputArray distCoeffs,
|
||||
OutputArray imagePoints,
|
||||
OutputArray jacobian = noArray(),
|
||||
double aspectRatio = 0 );
|
||||
|
||||
|
||||
void _drawPlanarBoard(Board *_board, Size outSize, OutputArray _img, int marginSize,
|
||||
int borderBits, bool drawAxis) {
|
||||
|
||||
@@ -142,35 +127,194 @@ void _drawPlanarBoard(Board *_board, Size outSize, OutputArray _img, int marginS
|
||||
}
|
||||
}
|
||||
|
||||
/* Draw a (potentially partially visible) line. */
|
||||
static void linePartial(InputOutputArray image, Point3f p1, Point3f p2, const Scalar& color,
|
||||
int thickness = 1, int lineType = LINE_8, int shift = 0)
|
||||
/**
|
||||
* @brief Convert point coordinates from world space to camera space.
|
||||
*
|
||||
* @param points A vector of points in world space.
|
||||
* @param rvec Rotation matrix or Rodrigues rotation vector.
|
||||
* @param tvec Translation vector from world to camera space.
|
||||
*
|
||||
* @return A vector of points in camera space.
|
||||
*/
|
||||
template<typename CvPointType>
|
||||
static std::vector<CvPointType> worldToCamera(const std::vector<CvPointType>& points,
|
||||
const cv::Mat& rvec, const cv::Mat& tvec)
|
||||
{
|
||||
// If both points are behind the screen, don't draw anything
|
||||
if (p1.z <= 0 && p2.z <= 0) {
|
||||
return;
|
||||
// We operate with CV_64F matrices internally to avoid precision loss
|
||||
cv::Mat rvec_64f;
|
||||
cv::Mat tvec_64f;
|
||||
rvec.convertTo(rvec_64f, CV_64F);
|
||||
tvec.convertTo(tvec_64f, CV_64F);
|
||||
|
||||
// Convert Rodrigues vector to rotation matrix
|
||||
cv::Mat rmat;
|
||||
if ((rvec_64f.cols == 3 && rvec_64f.rows == 1) ||
|
||||
(rvec_64f.cols == 1 && rvec_64f.rows == 3))
|
||||
{
|
||||
Rodrigues(rvec_64f, rmat);
|
||||
}
|
||||
Point2f p1p{p1.x, p1.y};
|
||||
Point2f p2p{p2.x, p2.y};
|
||||
// If points are on the different sides of the plane, compute intersection point
|
||||
if (p1.z * p2.z < 0) {
|
||||
// Compute intersection point with the screen
|
||||
// We denote alpha as such:
|
||||
// xi = (1 - alpha) * x1 + alpha * x2
|
||||
// yi = (1 - alpha) * y1 + alpha * y2
|
||||
// zi = (1 - alpha) * z1 + alpha * z2 = 0
|
||||
// Thus, alpha can be expressed as
|
||||
// alpha = z1 / (z1 - z2)
|
||||
float alpha = p1.z / (p1.z - p2.z);
|
||||
Point2f pi{(1 - alpha) * p1.x + alpha * p2.x, (1 - alpha) * p1.y + alpha * p2.y};
|
||||
// Now, if z1 is negative, we draw the line from (xi, yi) to (x2, y2), else we draw from (x1, y1) to (xi, yi)
|
||||
if (p1.z < 0) {
|
||||
p1p = pi;
|
||||
} else {
|
||||
p2p = pi;
|
||||
}
|
||||
else
|
||||
{
|
||||
rmat = rvec_64f.clone();
|
||||
}
|
||||
line(image, p1p, p2p, color, thickness, lineType, shift);
|
||||
// Make sure tvec has a size of (3, 1)
|
||||
if (tvec_64f.rows == 1)
|
||||
{
|
||||
tvec_64f = tvec_64f.t();
|
||||
}
|
||||
std::vector<CvPointType> result;
|
||||
result.reserve(points.size());
|
||||
for(const auto& point : points)
|
||||
{
|
||||
// Calculate point coordinates in camera frame
|
||||
// static_casts are here to silence potential narrowing conversion warnings
|
||||
CvPointType camPoint{
|
||||
static_cast<decltype(CvPointType::x)>(point.x * rmat.at<double>(0,0) + point.y * rmat.at<double>(0,1) + point.z * rmat.at<double>(0,2) + tvec_64f.at<double>(0)),
|
||||
static_cast<decltype(CvPointType::y)>(point.x * rmat.at<double>(1,0) + point.y * rmat.at<double>(1,1) + point.z * rmat.at<double>(1,2) + tvec_64f.at<double>(1)),
|
||||
static_cast<decltype(CvPointType::z)>(point.x * rmat.at<double>(2,0) + point.y * rmat.at<double>(2,1) + point.z * rmat.at<double>(2,2) + tvec_64f.at<double>(2))
|
||||
};
|
||||
result.push_back(camPoint);
|
||||
}
|
||||
return result;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Project points from camera space to screen space, applying distortion in the process.
|
||||
*
|
||||
* @param points A vector of points in camera space.
|
||||
* @param cameraMatrix OpenCV intrinsic camera matrix.
|
||||
* @param distCoeffs OpenCV distortion model coefficients.
|
||||
*
|
||||
* @return A vector of points in screen space.
|
||||
*/
|
||||
template<typename CvPointType>
|
||||
static std::vector<CvPointType> cameraToScreen(const std::vector<CvPointType>& points,
|
||||
const cv::Mat& cameraMatrix,
|
||||
const cv::Mat& distCoeffs)
|
||||
{
|
||||
// We operate with CV_64F matrices internally to avoid precision loss
|
||||
cv::Mat cm_64f; // camera matrix, CV_64F
|
||||
cv::Mat dc_64f; // distortion coefficients, CV_64F
|
||||
cameraMatrix.convertTo(cm_64f, CV_64F);
|
||||
distCoeffs.convertTo(dc_64f, CV_64F);
|
||||
|
||||
// Make sure distortion vector has a size of (N, 1)
|
||||
if (dc_64f.rows == 1)
|
||||
{
|
||||
dc_64f = dc_64f.t();
|
||||
}
|
||||
|
||||
// We will always use 12 distortion coefficients,
|
||||
// and we can safely pad missing ones with zeroes
|
||||
dc_64f.resize(12, 0.0);
|
||||
|
||||
std::vector<CvPointType> result;
|
||||
result.reserve(points.size());
|
||||
|
||||
for(const auto& point : points)
|
||||
{
|
||||
// Apply perspective projection, preserving initial Z coordinate
|
||||
// Always use double-precision
|
||||
cv::Point3d camPoint{
|
||||
point.x / point.z,
|
||||
point.y / point.z,
|
||||
point.z
|
||||
};
|
||||
|
||||
// Apply distortion
|
||||
// Note that we do not consider tilted sensor distortion
|
||||
// r^2 - distance from the image center squared
|
||||
double r2 = camPoint.x * camPoint.x + camPoint.y * camPoint.y;
|
||||
// r^4 - same, but to the 4th power
|
||||
double r4 = r2 * r2;
|
||||
// r^6 - same, but to the 6th power
|
||||
double r6 = r4 * r2;
|
||||
// tg1 - first tangential shift factor (2 * x * y)
|
||||
double tg1 = 2 * camPoint.x * camPoint.y;
|
||||
// tg2 - second tangential shift factor (r^2 + 2 * x^2)
|
||||
double tg2 = r2 + 2 * camPoint.x * camPoint.x;
|
||||
// tg3 - third tangential shift factor (r^2 + 2 * y^2)
|
||||
double tg3 = r2 + 2 * camPoint.y * camPoint.y;
|
||||
// polynomial distortion factor (numerator)
|
||||
double pndist = 1 + dc_64f.at<double>(0) * r2 + dc_64f.at<double>(1) * r4 + dc_64f.at<double>(4) * r6;
|
||||
// polynomial distortion factror (denominator)
|
||||
double pddist = 1.0 / (1 + dc_64f.at<double>(5) * r2 + dc_64f.at<double>(6) * r4 + dc_64f.at<double>(7) * r6);
|
||||
// Distorted point coordinates (always double-precision)
|
||||
cv::Point3d distortedPoint{
|
||||
camPoint.x * pndist * pddist + dc_64f.at<double>(2) * tg1 + dc_64f.at<double>(3) * tg2 + dc_64f.at<double>(8) * r2 + dc_64f.at<double>(9) * r4,
|
||||
camPoint.y * pndist * pddist + dc_64f.at<double>(2) * tg3 + dc_64f.at<double>(3) * tg1 + dc_64f.at<double>(10) * r2 + dc_64f.at<double>(11) * r4,
|
||||
camPoint.z
|
||||
};
|
||||
|
||||
// Convert to screen space
|
||||
// We use static_cast here to silence potential warnings about narrowing conversions
|
||||
// (we expect that to be the case)
|
||||
CvPointType screenPoint{
|
||||
static_cast<decltype(CvPointType::x)>(distortedPoint.x * cm_64f.at<double>(0, 0) + cm_64f.at<double>(0, 2)),
|
||||
static_cast<decltype(CvPointType::y)>(distortedPoint.y * cm_64f.at<double>(1, 1) + cm_64f.at<double>(1, 2)),
|
||||
static_cast<decltype(CvPointType::z)>(distortedPoint.z)
|
||||
};
|
||||
|
||||
result.push_back(screenPoint);
|
||||
}
|
||||
return result;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Clip a line against a clip plane.
|
||||
*
|
||||
* This function "clips" a line (described by two points in *camera space*)
|
||||
* against a clip plane that is `clipPlaneDistance` meters away from the
|
||||
* camera focal point. If both points are further away from the focal point
|
||||
* than `clipPlaneDistance`, they will be returned unmodified. If one of the
|
||||
* points is behind the clipping plane, a point *on* the clipping plane will
|
||||
* be computed and returned as one of the points.
|
||||
*
|
||||
* If none of the points are visible, an empty vector will be returned.
|
||||
*
|
||||
* @param p1 First point on the line, in camera space.
|
||||
* @param p2 Second point on the line, in camera space.
|
||||
* @param clipPlaneDistance Distance from the focal point to the clipping plane.
|
||||
* @return A vector of zero or two points on the clipped line, in camera space.
|
||||
*/
|
||||
static std::vector<Point3f> lineClip(Point3f p1, Point3f p2, float clipPlaneDistance = 0.1f)
|
||||
{
|
||||
// We don't need to compute an intersection if both points are
|
||||
// behind us
|
||||
if (p1.z < clipPlaneDistance && p2.z < clipPlaneDistance)
|
||||
{
|
||||
return {};
|
||||
}
|
||||
// We don't need to compute an intersection if both points are
|
||||
// in front of us
|
||||
if (p1.z > clipPlaneDistance && p2.z > clipPlaneDistance)
|
||||
{
|
||||
return {p1, p2};
|
||||
}
|
||||
// We don't really want to compute an intersection if both Z coordinates
|
||||
// are sufficiently close to each other
|
||||
if (std::abs(p1.z - p2.z) < 0.0001) // The number here is chosen arbitrarily
|
||||
{
|
||||
return {p1, p2};
|
||||
}
|
||||
// We compute the intersection as such:
|
||||
// zi = (1 - alpha) * p1.z + alpha * p2.z = clipPlaneDistance
|
||||
// alpha = (p1.z - clipPlaneDistance) / (p1.z - p2.z)
|
||||
double alpha = (p1.z - clipPlaneDistance) / (p1.z - p2.z);
|
||||
Point3f clipPlanePoint{
|
||||
static_cast<float>((1 - alpha) * p1.x + alpha * p2.x),
|
||||
static_cast<float>((1 - alpha) * p1.y + alpha * p2.y),
|
||||
clipPlaneDistance
|
||||
};
|
||||
if (p1.z < clipPlaneDistance)
|
||||
{
|
||||
return {clipPlanePoint, p2};
|
||||
}
|
||||
else
|
||||
{
|
||||
return {p1, clipPlanePoint};
|
||||
}
|
||||
// Unreachable?
|
||||
}
|
||||
|
||||
void _drawAxis(InputOutputArray _image, InputArray _cameraMatrix, InputArray _distCoeffs,
|
||||
@@ -186,647 +330,23 @@ void _drawAxis(InputOutputArray _image, InputArray _cameraMatrix, InputArray _di
|
||||
axisPoints.push_back(Point3f(length, 0, 0));
|
||||
axisPoints.push_back(Point3f(0, length, 0));
|
||||
axisPoints.push_back(Point3f(0, 0, length));
|
||||
std::vector<Point3f> imagePointsZ;
|
||||
_projectPoints(axisPoints, _rvec, _tvec, _cameraMatrix, _distCoeffs, imagePointsZ);
|
||||
|
||||
// draw axis lines
|
||||
linePartial(_image, imagePointsZ[0], imagePointsZ[1], Scalar(0, 0, 255), 3);
|
||||
linePartial(_image, imagePointsZ[0], imagePointsZ[2], Scalar(0, 255, 0), 3);
|
||||
linePartial(_image, imagePointsZ[0], imagePointsZ[3], Scalar(255, 0, 0), 3);
|
||||
}
|
||||
|
||||
static CvMat _cvMat(const cv::Mat& m)
|
||||
{
|
||||
CvMat self;
|
||||
CV_DbgAssert(m.dims <= 2);
|
||||
self = cvMat(m.rows, m.dims == 1 ? 1 : m.cols, m.type(), m.data);
|
||||
self.step = (int)m.step[0];
|
||||
self.type = (self.type & ~cv::Mat::CONTINUOUS_FLAG) | (m.flags & cv::Mat::CONTINUOUS_FLAG);
|
||||
return self;
|
||||
}
|
||||
|
||||
static void _projectPoints( InputArray _opoints,
|
||||
InputArray _rvec,
|
||||
InputArray _tvec,
|
||||
InputArray _cameraMatrix,
|
||||
InputArray _distCoeffs,
|
||||
OutputArray _ipoints,
|
||||
OutputArray _jacobian,
|
||||
double aspectRatio )
|
||||
{
|
||||
Mat opoints = _opoints.getMat();
|
||||
int npoints = opoints.checkVector(3), depth = opoints.depth();
|
||||
CV_Assert(npoints >= 0 && (depth == CV_32F || depth == CV_64F));
|
||||
|
||||
CvMat dpdrot, dpdt, dpdf, dpdc, dpddist;
|
||||
CvMat *pdpdrot = 0, *pdpdt = 0, *pdpdf = 0, *pdpdc = 0, *pdpddist = 0;
|
||||
|
||||
CV_Assert(_ipoints.needed());
|
||||
|
||||
_ipoints.create(npoints, 1, CV_MAKETYPE(depth, 3), -1, true);
|
||||
Mat imagePoints = _ipoints.getMat();
|
||||
CvMat c_imagePoints = _cvMat(imagePoints);
|
||||
CvMat c_objectPoints = _cvMat(opoints);
|
||||
Mat cameraMatrix = _cameraMatrix.getMat();
|
||||
|
||||
Mat rvec = _rvec.getMat(), tvec = _tvec.getMat();
|
||||
CvMat c_cameraMatrix = _cvMat(cameraMatrix);
|
||||
CvMat c_rvec = _cvMat(rvec), c_tvec = _cvMat(tvec);
|
||||
|
||||
double dc0buf[5] = {0};
|
||||
Mat dc0(5, 1, CV_64F, dc0buf);
|
||||
Mat distCoeffs = _distCoeffs.getMat();
|
||||
if (distCoeffs.empty())
|
||||
distCoeffs = dc0;
|
||||
CvMat c_distCoeffs = _cvMat(distCoeffs);
|
||||
int ndistCoeffs = distCoeffs.rows + distCoeffs.cols - 1;
|
||||
|
||||
Mat jacobian;
|
||||
if (_jacobian.needed())
|
||||
auto camAxisPoints = worldToCamera(axisPoints, _rvec.getMat(), _tvec.getMat());
|
||||
auto axisX = cameraToScreen(lineClip(camAxisPoints[0], camAxisPoints[1]), _cameraMatrix.getMat(), _distCoeffs.getMat());
|
||||
auto axisY = cameraToScreen(lineClip(camAxisPoints[0], camAxisPoints[2]), _cameraMatrix.getMat(), _distCoeffs.getMat());
|
||||
auto axisZ = cameraToScreen(lineClip(camAxisPoints[0], camAxisPoints[3]), _cameraMatrix.getMat(), _distCoeffs.getMat());
|
||||
if (axisX.size() > 0)
|
||||
{
|
||||
_jacobian.create(npoints * 2, 3 + 3 + 2 + 2 + ndistCoeffs, CV_64F);
|
||||
jacobian = _jacobian.getMat();
|
||||
pdpdrot = &(dpdrot = _cvMat(jacobian.colRange(0, 3)));
|
||||
pdpdt = &(dpdt = _cvMat(jacobian.colRange(3, 6)));
|
||||
pdpdf = &(dpdf = _cvMat(jacobian.colRange(6, 8)));
|
||||
pdpdc = &(dpdc = _cvMat(jacobian.colRange(8, 10)));
|
||||
pdpddist = &(dpddist = _cvMat(jacobian.colRange(10, 10 + ndistCoeffs)));
|
||||
line(_image, Point2f{axisX[0].x, axisX[0].y}, Point2f{axisX[1].x, axisX[1].y},
|
||||
Scalar(0, 0, 255), 3);
|
||||
}
|
||||
if (axisY.size() > 0)
|
||||
{
|
||||
line(_image, Point2f{axisY[0].x, axisY[0].y}, Point2f{axisY[1].x, axisY[1].y},
|
||||
Scalar(0, 255, 0), 3);
|
||||
}
|
||||
if (axisZ.size() > 0)
|
||||
{
|
||||
line(_image, Point2f{axisZ[0].x, axisZ[0].y}, Point2f{axisZ[1].x, axisZ[1].y},
|
||||
Scalar(255, 0, 0), 3);
|
||||
}
|
||||
|
||||
_cvProjectPoints2(&c_objectPoints, &c_rvec, &c_tvec, &c_cameraMatrix, &c_distCoeffs,
|
||||
&c_imagePoints, pdpdrot, pdpdt, pdpdf, pdpdc, pdpddist, aspectRatio);
|
||||
}
|
||||
|
||||
namespace _detail
|
||||
{
|
||||
template <typename FLOAT>
|
||||
void computeTiltProjectionMatrix(FLOAT tauX,
|
||||
FLOAT tauY,
|
||||
Matx<FLOAT, 3, 3>* matTilt = 0,
|
||||
Matx<FLOAT, 3, 3>* dMatTiltdTauX = 0,
|
||||
Matx<FLOAT, 3, 3>* dMatTiltdTauY = 0,
|
||||
Matx<FLOAT, 3, 3>* invMatTilt = 0)
|
||||
{
|
||||
FLOAT cTauX = cos(tauX);
|
||||
FLOAT sTauX = sin(tauX);
|
||||
FLOAT cTauY = cos(tauY);
|
||||
FLOAT sTauY = sin(tauY);
|
||||
Matx<FLOAT, 3, 3> matRotX = Matx<FLOAT, 3, 3>(1,0,0,0,cTauX,sTauX,0,-sTauX,cTauX);
|
||||
Matx<FLOAT, 3, 3> matRotY = Matx<FLOAT, 3, 3>(cTauY,0,-sTauY,0,1,0,sTauY,0,cTauY);
|
||||
Matx<FLOAT, 3, 3> matRotXY = matRotY * matRotX;
|
||||
Matx<FLOAT, 3, 3> matProjZ = Matx<FLOAT, 3, 3>(matRotXY(2,2),0,-matRotXY(0,2),0,matRotXY(2,2),-matRotXY(1,2),0,0,1);
|
||||
if (matTilt)
|
||||
{
|
||||
// Matrix for trapezoidal distortion of tilted image sensor
|
||||
*matTilt = matProjZ * matRotXY;
|
||||
}
|
||||
if (dMatTiltdTauX)
|
||||
{
|
||||
// Derivative with respect to tauX
|
||||
Matx<FLOAT, 3, 3> dMatRotXYdTauX = matRotY * Matx<FLOAT, 3, 3>(0,0,0,0,-sTauX,cTauX,0,-cTauX,-sTauX);
|
||||
Matx<FLOAT, 3, 3> dMatProjZdTauX = Matx<FLOAT, 3, 3>(dMatRotXYdTauX(2,2),0,-dMatRotXYdTauX(0,2),
|
||||
0,dMatRotXYdTauX(2,2),-dMatRotXYdTauX(1,2),0,0,0);
|
||||
*dMatTiltdTauX = (matProjZ * dMatRotXYdTauX) + (dMatProjZdTauX * matRotXY);
|
||||
}
|
||||
if (dMatTiltdTauY)
|
||||
{
|
||||
// Derivative with respect to tauY
|
||||
Matx<FLOAT, 3, 3> dMatRotXYdTauY = Matx<FLOAT, 3, 3>(-sTauY,0,-cTauY,0,0,0,cTauY,0,-sTauY) * matRotX;
|
||||
Matx<FLOAT, 3, 3> dMatProjZdTauY = Matx<FLOAT, 3, 3>(dMatRotXYdTauY(2,2),0,-dMatRotXYdTauY(0,2),
|
||||
0,dMatRotXYdTauY(2,2),-dMatRotXYdTauY(1,2),0,0,0);
|
||||
*dMatTiltdTauY = (matProjZ * dMatRotXYdTauY) + (dMatProjZdTauY * matRotXY);
|
||||
}
|
||||
if (invMatTilt)
|
||||
{
|
||||
FLOAT inv = 1./matRotXY(2,2);
|
||||
Matx<FLOAT, 3, 3> invMatProjZ = Matx<FLOAT, 3, 3>(inv,0,inv*matRotXY(0,2),0,inv,inv*matRotXY(1,2),0,0,1);
|
||||
*invMatTilt = matRotXY.t()*invMatProjZ;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
static const char* cvDistCoeffErr = "Distortion coefficients must be 1x4, 4x1, 1x5, 5x1, 1x8, 8x1, 1x12, 12x1, 1x14 or 14x1 floating-point vector";
|
||||
|
||||
static void _cvProjectPoints2Internal( const CvMat* objectPoints,
|
||||
const CvMat* r_vec,
|
||||
const CvMat* t_vec,
|
||||
const CvMat* A,
|
||||
const CvMat* distCoeffs,
|
||||
CvMat* imagePoints, CvMat* dpdr CV_DEFAULT(NULL),
|
||||
CvMat* dpdt CV_DEFAULT(NULL), CvMat* dpdf CV_DEFAULT(NULL),
|
||||
CvMat* dpdc CV_DEFAULT(NULL), CvMat* dpdk CV_DEFAULT(NULL),
|
||||
CvMat* dpdo CV_DEFAULT(NULL),
|
||||
double aspectRatio CV_DEFAULT(0) )
|
||||
{
|
||||
Ptr<CvMat> matM, _m;
|
||||
Ptr<CvMat> _dpdr, _dpdt, _dpdc, _dpdf, _dpdk;
|
||||
Ptr<CvMat> _dpdo;
|
||||
|
||||
int i, j, count;
|
||||
int calc_derivatives;
|
||||
const CvPoint3D64f* M;
|
||||
CvPoint3D64f* m;
|
||||
double r[3], R[9], dRdr[27], t[3], a[9], k[14] = {0,0,0,0,0,0,0,0,0,0,0,0,0,0}, fx, fy, cx, cy;
|
||||
Matx33d matTilt = Matx33d::eye();
|
||||
Matx33d dMatTiltdTauX(0,0,0,0,0,0,0,-1,0);
|
||||
Matx33d dMatTiltdTauY(0,0,0,0,0,0,1,0,0);
|
||||
CvMat _r, _t, _a = cvMat( 3, 3, CV_64F, a ), _k;
|
||||
CvMat matR = cvMat( 3, 3, CV_64F, R ), _dRdr = cvMat( 3, 9, CV_64F, dRdr );
|
||||
double *dpdr_p = 0, *dpdt_p = 0, *dpdk_p = 0, *dpdf_p = 0, *dpdc_p = 0;
|
||||
double* dpdo_p = 0;
|
||||
int dpdr_step = 0, dpdt_step = 0, dpdk_step = 0, dpdf_step = 0, dpdc_step = 0;
|
||||
int dpdo_step = 0;
|
||||
bool fixedAspectRatio = aspectRatio > FLT_EPSILON;
|
||||
|
||||
if( !CV_IS_MAT(objectPoints) || !CV_IS_MAT(r_vec) ||
|
||||
!CV_IS_MAT(t_vec) || !CV_IS_MAT(A) ||
|
||||
/*!CV_IS_MAT(distCoeffs) ||*/ !CV_IS_MAT(imagePoints) )
|
||||
CV_Error( CV_StsBadArg, "One of required arguments is not a valid matrix" );
|
||||
|
||||
int total = objectPoints->rows * objectPoints->cols * CV_MAT_CN(objectPoints->type);
|
||||
if(total % 3 != 0)
|
||||
{
|
||||
//we have stopped support of homogeneous coordinates because it cause ambiguity in interpretation of the input data
|
||||
CV_Error( CV_StsBadArg, "Homogeneous coordinates are not supported" );
|
||||
}
|
||||
count = total / 3;
|
||||
|
||||
if( CV_IS_CONT_MAT(objectPoints->type) &&
|
||||
(CV_MAT_DEPTH(objectPoints->type) == CV_32F || CV_MAT_DEPTH(objectPoints->type) == CV_64F)&&
|
||||
((objectPoints->rows == 1 && CV_MAT_CN(objectPoints->type) == 3) ||
|
||||
(objectPoints->rows == count && CV_MAT_CN(objectPoints->type)*objectPoints->cols == 3) ||
|
||||
(objectPoints->rows == 3 && CV_MAT_CN(objectPoints->type) == 1 && objectPoints->cols == count)))
|
||||
{
|
||||
matM.reset(cvCreateMat( objectPoints->rows, objectPoints->cols, CV_MAKETYPE(CV_64F,CV_MAT_CN(objectPoints->type)) ));
|
||||
cvConvert(objectPoints, matM);
|
||||
}
|
||||
else
|
||||
{
|
||||
// matM = cvCreateMat( 1, count, CV_64FC3 );
|
||||
// cvConvertPointsHomogeneous( objectPoints, matM );
|
||||
CV_Error( CV_StsBadArg, "Homogeneous coordinates are not supported" );
|
||||
}
|
||||
|
||||
if( CV_IS_CONT_MAT(imagePoints->type) &&
|
||||
(CV_MAT_DEPTH(imagePoints->type) == CV_32F || CV_MAT_DEPTH(imagePoints->type) == CV_64F) &&
|
||||
((imagePoints->rows == 1 && CV_MAT_CN(imagePoints->type) == 3) ||
|
||||
(imagePoints->rows == count && CV_MAT_CN(imagePoints->type)*imagePoints->cols == 3) ||
|
||||
(imagePoints->rows == 3 && CV_MAT_CN(imagePoints->type) == 1 && imagePoints->cols == count)))
|
||||
{
|
||||
_m.reset(cvCreateMat( imagePoints->rows, imagePoints->cols, CV_MAKETYPE(CV_64F,CV_MAT_CN(imagePoints->type)) ));
|
||||
cvConvert(imagePoints, _m);
|
||||
}
|
||||
else
|
||||
{
|
||||
// _m = cvCreateMat( 1, count, CV_64FC2 );
|
||||
CV_Error( CV_StsBadArg, "Homogeneous coordinates are not supported" );
|
||||
}
|
||||
|
||||
M = (CvPoint3D64f*)matM->data.db;
|
||||
m = (CvPoint3D64f*)_m->data.db;
|
||||
|
||||
if( (CV_MAT_DEPTH(r_vec->type) != CV_64F && CV_MAT_DEPTH(r_vec->type) != CV_32F) ||
|
||||
(((r_vec->rows != 1 && r_vec->cols != 1) ||
|
||||
r_vec->rows*r_vec->cols*CV_MAT_CN(r_vec->type) != 3) &&
|
||||
((r_vec->rows != 3 && r_vec->cols != 3) || CV_MAT_CN(r_vec->type) != 1)))
|
||||
CV_Error( CV_StsBadArg, "Rotation must be represented by 1x3 or 3x1 "
|
||||
"floating-point rotation vector, or 3x3 rotation matrix" );
|
||||
|
||||
if( r_vec->rows == 3 && r_vec->cols == 3 )
|
||||
{
|
||||
_r = cvMat( 3, 1, CV_64FC1, r );
|
||||
cvRodrigues2( r_vec, &_r );
|
||||
cvRodrigues2( &_r, &matR, &_dRdr );
|
||||
cvCopy( r_vec, &matR );
|
||||
}
|
||||
else
|
||||
{
|
||||
_r = cvMat( r_vec->rows, r_vec->cols, CV_MAKETYPE(CV_64F,CV_MAT_CN(r_vec->type)), r );
|
||||
cvConvert( r_vec, &_r );
|
||||
cvRodrigues2( &_r, &matR, &_dRdr );
|
||||
}
|
||||
|
||||
if( (CV_MAT_DEPTH(t_vec->type) != CV_64F && CV_MAT_DEPTH(t_vec->type) != CV_32F) ||
|
||||
(t_vec->rows != 1 && t_vec->cols != 1) ||
|
||||
t_vec->rows*t_vec->cols*CV_MAT_CN(t_vec->type) != 3 )
|
||||
CV_Error( CV_StsBadArg,
|
||||
"Translation vector must be 1x3 or 3x1 floating-point vector" );
|
||||
|
||||
_t = cvMat( t_vec->rows, t_vec->cols, CV_MAKETYPE(CV_64F,CV_MAT_CN(t_vec->type)), t );
|
||||
cvConvert( t_vec, &_t );
|
||||
|
||||
if( (CV_MAT_TYPE(A->type) != CV_64FC1 && CV_MAT_TYPE(A->type) != CV_32FC1) ||
|
||||
A->rows != 3 || A->cols != 3 )
|
||||
CV_Error( CV_StsBadArg, "Instrinsic parameters must be 3x3 floating-point matrix" );
|
||||
|
||||
cvConvert( A, &_a );
|
||||
fx = a[0]; fy = a[4];
|
||||
cx = a[2]; cy = a[5];
|
||||
|
||||
if( fixedAspectRatio )
|
||||
fx = fy*aspectRatio;
|
||||
|
||||
if( distCoeffs )
|
||||
{
|
||||
if( !CV_IS_MAT(distCoeffs) ||
|
||||
(CV_MAT_DEPTH(distCoeffs->type) != CV_64F &&
|
||||
CV_MAT_DEPTH(distCoeffs->type) != CV_32F) ||
|
||||
(distCoeffs->rows != 1 && distCoeffs->cols != 1) ||
|
||||
(distCoeffs->rows*distCoeffs->cols*CV_MAT_CN(distCoeffs->type) != 4 &&
|
||||
distCoeffs->rows*distCoeffs->cols*CV_MAT_CN(distCoeffs->type) != 5 &&
|
||||
distCoeffs->rows*distCoeffs->cols*CV_MAT_CN(distCoeffs->type) != 8 &&
|
||||
distCoeffs->rows*distCoeffs->cols*CV_MAT_CN(distCoeffs->type) != 12 &&
|
||||
distCoeffs->rows*distCoeffs->cols*CV_MAT_CN(distCoeffs->type) != 14) )
|
||||
CV_Error( CV_StsBadArg, cvDistCoeffErr );
|
||||
|
||||
_k = cvMat( distCoeffs->rows, distCoeffs->cols,
|
||||
CV_MAKETYPE(CV_64F,CV_MAT_CN(distCoeffs->type)), k );
|
||||
cvConvert( distCoeffs, &_k );
|
||||
if(k[12] != 0 || k[13] != 0)
|
||||
{
|
||||
_detail::computeTiltProjectionMatrix(k[12], k[13],
|
||||
&matTilt, &dMatTiltdTauX, &dMatTiltdTauY);
|
||||
}
|
||||
}
|
||||
|
||||
if( dpdr )
|
||||
{
|
||||
if( !CV_IS_MAT(dpdr) ||
|
||||
(CV_MAT_TYPE(dpdr->type) != CV_32FC1 &&
|
||||
CV_MAT_TYPE(dpdr->type) != CV_64FC1) ||
|
||||
dpdr->rows != count*2 || dpdr->cols != 3 )
|
||||
CV_Error( CV_StsBadArg, "dp/drot must be 2Nx3 floating-point matrix" );
|
||||
|
||||
if( CV_MAT_TYPE(dpdr->type) == CV_64FC1 )
|
||||
{
|
||||
_dpdr.reset(cvCloneMat(dpdr));
|
||||
}
|
||||
else
|
||||
_dpdr.reset(cvCreateMat( 2*count, 3, CV_64FC1 ));
|
||||
dpdr_p = _dpdr->data.db;
|
||||
dpdr_step = _dpdr->step/sizeof(dpdr_p[0]);
|
||||
}
|
||||
|
||||
if( dpdt )
|
||||
{
|
||||
if( !CV_IS_MAT(dpdt) ||
|
||||
(CV_MAT_TYPE(dpdt->type) != CV_32FC1 &&
|
||||
CV_MAT_TYPE(dpdt->type) != CV_64FC1) ||
|
||||
dpdt->rows != count*2 || dpdt->cols != 3 )
|
||||
CV_Error( CV_StsBadArg, "dp/dT must be 2Nx3 floating-point matrix" );
|
||||
|
||||
if( CV_MAT_TYPE(dpdt->type) == CV_64FC1 )
|
||||
{
|
||||
_dpdt.reset(cvCloneMat(dpdt));
|
||||
}
|
||||
else
|
||||
_dpdt.reset(cvCreateMat( 2*count, 3, CV_64FC1 ));
|
||||
dpdt_p = _dpdt->data.db;
|
||||
dpdt_step = _dpdt->step/sizeof(dpdt_p[0]);
|
||||
}
|
||||
|
||||
if( dpdf )
|
||||
{
|
||||
if( !CV_IS_MAT(dpdf) ||
|
||||
(CV_MAT_TYPE(dpdf->type) != CV_32FC1 && CV_MAT_TYPE(dpdf->type) != CV_64FC1) ||
|
||||
dpdf->rows != count*2 || dpdf->cols != 2 )
|
||||
CV_Error( CV_StsBadArg, "dp/df must be 2Nx2 floating-point matrix" );
|
||||
|
||||
if( CV_MAT_TYPE(dpdf->type) == CV_64FC1 )
|
||||
{
|
||||
_dpdf.reset(cvCloneMat(dpdf));
|
||||
}
|
||||
else
|
||||
_dpdf.reset(cvCreateMat( 2*count, 2, CV_64FC1 ));
|
||||
dpdf_p = _dpdf->data.db;
|
||||
dpdf_step = _dpdf->step/sizeof(dpdf_p[0]);
|
||||
}
|
||||
|
||||
if( dpdc )
|
||||
{
|
||||
if( !CV_IS_MAT(dpdc) ||
|
||||
(CV_MAT_TYPE(dpdc->type) != CV_32FC1 && CV_MAT_TYPE(dpdc->type) != CV_64FC1) ||
|
||||
dpdc->rows != count*2 || dpdc->cols != 2 )
|
||||
CV_Error( CV_StsBadArg, "dp/dc must be 2Nx2 floating-point matrix" );
|
||||
|
||||
if( CV_MAT_TYPE(dpdc->type) == CV_64FC1 )
|
||||
{
|
||||
_dpdc.reset(cvCloneMat(dpdc));
|
||||
}
|
||||
else
|
||||
_dpdc.reset(cvCreateMat( 2*count, 2, CV_64FC1 ));
|
||||
dpdc_p = _dpdc->data.db;
|
||||
dpdc_step = _dpdc->step/sizeof(dpdc_p[0]);
|
||||
}
|
||||
|
||||
if( dpdk )
|
||||
{
|
||||
if( !CV_IS_MAT(dpdk) ||
|
||||
(CV_MAT_TYPE(dpdk->type) != CV_32FC1 && CV_MAT_TYPE(dpdk->type) != CV_64FC1) ||
|
||||
dpdk->rows != count*2 || (dpdk->cols != 14 && dpdk->cols != 12 && dpdk->cols != 8 && dpdk->cols != 5 && dpdk->cols != 4 && dpdk->cols != 2) )
|
||||
CV_Error( CV_StsBadArg, "dp/df must be 2Nx14, 2Nx12, 2Nx8, 2Nx5, 2Nx4 or 2Nx2 floating-point matrix" );
|
||||
|
||||
if( !distCoeffs )
|
||||
CV_Error( CV_StsNullPtr, "distCoeffs is NULL while dpdk is not" );
|
||||
|
||||
if( CV_MAT_TYPE(dpdk->type) == CV_64FC1 )
|
||||
{
|
||||
_dpdk.reset(cvCloneMat(dpdk));
|
||||
}
|
||||
else
|
||||
_dpdk.reset(cvCreateMat( dpdk->rows, dpdk->cols, CV_64FC1 ));
|
||||
dpdk_p = _dpdk->data.db;
|
||||
dpdk_step = _dpdk->step/sizeof(dpdk_p[0]);
|
||||
}
|
||||
|
||||
if( dpdo )
|
||||
{
|
||||
if( !CV_IS_MAT( dpdo ) || ( CV_MAT_TYPE( dpdo->type ) != CV_32FC1
|
||||
&& CV_MAT_TYPE( dpdo->type ) != CV_64FC1 )
|
||||
|| dpdo->rows != count * 2 || dpdo->cols != count * 3 )
|
||||
CV_Error( CV_StsBadArg, "dp/do must be 2Nx3N floating-point matrix" );
|
||||
|
||||
if( CV_MAT_TYPE( dpdo->type ) == CV_64FC1 )
|
||||
{
|
||||
_dpdo.reset( cvCloneMat( dpdo ) );
|
||||
}
|
||||
else
|
||||
_dpdo.reset( cvCreateMat( 2 * count, 3 * count, CV_64FC1 ) );
|
||||
cvZero(_dpdo);
|
||||
dpdo_p = _dpdo->data.db;
|
||||
dpdo_step = _dpdo->step / sizeof( dpdo_p[0] );
|
||||
}
|
||||
|
||||
calc_derivatives = dpdr || dpdt || dpdf || dpdc || dpdk || dpdo;
|
||||
|
||||
for( i = 0; i < count; i++ )
|
||||
{
|
||||
double X = M[i].x, Y = M[i].y, Z = M[i].z;
|
||||
double x = R[0]*X + R[1]*Y + R[2]*Z + t[0];
|
||||
double y = R[3]*X + R[4]*Y + R[5]*Z + t[1];
|
||||
double z = R[6]*X + R[7]*Y + R[8]*Z + t[2];
|
||||
double r2, r4, r6, a1, a2, a3, cdist, icdist2;
|
||||
double xd, yd, xd0, yd0, invProj;
|
||||
Vec3d vecTilt;
|
||||
Vec3d dVecTilt;
|
||||
Matx22d dMatTilt;
|
||||
Vec2d dXdYd;
|
||||
|
||||
double z0 = z;
|
||||
z = z ? 1./z : 1;
|
||||
x *= z; y *= z;
|
||||
|
||||
r2 = x*x + y*y;
|
||||
r4 = r2*r2;
|
||||
r6 = r4*r2;
|
||||
a1 = 2*x*y;
|
||||
a2 = r2 + 2*x*x;
|
||||
a3 = r2 + 2*y*y;
|
||||
cdist = 1 + k[0]*r2 + k[1]*r4 + k[4]*r6;
|
||||
icdist2 = 1./(1 + k[5]*r2 + k[6]*r4 + k[7]*r6);
|
||||
xd0 = x*cdist*icdist2 + k[2]*a1 + k[3]*a2 + k[8]*r2+k[9]*r4;
|
||||
yd0 = y*cdist*icdist2 + k[2]*a3 + k[3]*a1 + k[10]*r2+k[11]*r4;
|
||||
|
||||
// additional distortion by projecting onto a tilt plane
|
||||
vecTilt = matTilt*Vec3d(xd0, yd0, 1);
|
||||
invProj = vecTilt(2) ? 1./vecTilt(2) : 1;
|
||||
xd = invProj * vecTilt(0);
|
||||
yd = invProj * vecTilt(1);
|
||||
|
||||
m[i].x = xd*fx + cx;
|
||||
m[i].y = yd*fy + cy;
|
||||
m[i].z = z; // Just put the projected Z coordinate here, we mainly care about the sign
|
||||
|
||||
if( calc_derivatives )
|
||||
{
|
||||
if( dpdc_p )
|
||||
{
|
||||
dpdc_p[0] = 1; dpdc_p[1] = 0; // dp_xdc_x; dp_xdc_y
|
||||
dpdc_p[dpdc_step] = 0;
|
||||
dpdc_p[dpdc_step+1] = 1;
|
||||
dpdc_p += dpdc_step*2;
|
||||
}
|
||||
|
||||
if( dpdf_p )
|
||||
{
|
||||
if( fixedAspectRatio )
|
||||
{
|
||||
dpdf_p[0] = 0; dpdf_p[1] = xd*aspectRatio; // dp_xdf_x; dp_xdf_y
|
||||
dpdf_p[dpdf_step] = 0;
|
||||
dpdf_p[dpdf_step+1] = yd;
|
||||
}
|
||||
else
|
||||
{
|
||||
dpdf_p[0] = xd; dpdf_p[1] = 0;
|
||||
dpdf_p[dpdf_step] = 0;
|
||||
dpdf_p[dpdf_step+1] = yd;
|
||||
}
|
||||
dpdf_p += dpdf_step*2;
|
||||
}
|
||||
for (int row = 0; row < 2; ++row)
|
||||
for (int col = 0; col < 2; ++col)
|
||||
dMatTilt(row,col) = matTilt(row,col)*vecTilt(2)
|
||||
- matTilt(2,col)*vecTilt(row);
|
||||
double invProjSquare = (invProj*invProj);
|
||||
dMatTilt *= invProjSquare;
|
||||
if( dpdk_p )
|
||||
{
|
||||
dXdYd = dMatTilt*Vec2d(x*icdist2*r2, y*icdist2*r2);
|
||||
dpdk_p[0] = fx*dXdYd(0);
|
||||
dpdk_p[dpdk_step] = fy*dXdYd(1);
|
||||
dXdYd = dMatTilt*Vec2d(x*icdist2*r4, y*icdist2*r4);
|
||||
dpdk_p[1] = fx*dXdYd(0);
|
||||
dpdk_p[dpdk_step+1] = fy*dXdYd(1);
|
||||
if( _dpdk->cols > 2 )
|
||||
{
|
||||
dXdYd = dMatTilt*Vec2d(a1, a3);
|
||||
dpdk_p[2] = fx*dXdYd(0);
|
||||
dpdk_p[dpdk_step+2] = fy*dXdYd(1);
|
||||
dXdYd = dMatTilt*Vec2d(a2, a1);
|
||||
dpdk_p[3] = fx*dXdYd(0);
|
||||
dpdk_p[dpdk_step+3] = fy*dXdYd(1);
|
||||
if( _dpdk->cols > 4 )
|
||||
{
|
||||
dXdYd = dMatTilt*Vec2d(x*icdist2*r6, y*icdist2*r6);
|
||||
dpdk_p[4] = fx*dXdYd(0);
|
||||
dpdk_p[dpdk_step+4] = fy*dXdYd(1);
|
||||
|
||||
if( _dpdk->cols > 5 )
|
||||
{
|
||||
dXdYd = dMatTilt*Vec2d(
|
||||
x*cdist*(-icdist2)*icdist2*r2, y*cdist*(-icdist2)*icdist2*r2);
|
||||
dpdk_p[5] = fx*dXdYd(0);
|
||||
dpdk_p[dpdk_step+5] = fy*dXdYd(1);
|
||||
dXdYd = dMatTilt*Vec2d(
|
||||
x*cdist*(-icdist2)*icdist2*r4, y*cdist*(-icdist2)*icdist2*r4);
|
||||
dpdk_p[6] = fx*dXdYd(0);
|
||||
dpdk_p[dpdk_step+6] = fy*dXdYd(1);
|
||||
dXdYd = dMatTilt*Vec2d(
|
||||
x*cdist*(-icdist2)*icdist2*r6, y*cdist*(-icdist2)*icdist2*r6);
|
||||
dpdk_p[7] = fx*dXdYd(0);
|
||||
dpdk_p[dpdk_step+7] = fy*dXdYd(1);
|
||||
if( _dpdk->cols > 8 )
|
||||
{
|
||||
dXdYd = dMatTilt*Vec2d(r2, 0);
|
||||
dpdk_p[8] = fx*dXdYd(0); //s1
|
||||
dpdk_p[dpdk_step+8] = fy*dXdYd(1); //s1
|
||||
dXdYd = dMatTilt*Vec2d(r4, 0);
|
||||
dpdk_p[9] = fx*dXdYd(0); //s2
|
||||
dpdk_p[dpdk_step+9] = fy*dXdYd(1); //s2
|
||||
dXdYd = dMatTilt*Vec2d(0, r2);
|
||||
dpdk_p[10] = fx*dXdYd(0);//s3
|
||||
dpdk_p[dpdk_step+10] = fy*dXdYd(1); //s3
|
||||
dXdYd = dMatTilt*Vec2d(0, r4);
|
||||
dpdk_p[11] = fx*dXdYd(0);//s4
|
||||
dpdk_p[dpdk_step+11] = fy*dXdYd(1); //s4
|
||||
if( _dpdk->cols > 12 )
|
||||
{
|
||||
dVecTilt = dMatTiltdTauX * Vec3d(xd0, yd0, 1);
|
||||
dpdk_p[12] = fx * invProjSquare * (
|
||||
dVecTilt(0) * vecTilt(2) - dVecTilt(2) * vecTilt(0));
|
||||
dpdk_p[dpdk_step+12] = fy*invProjSquare * (
|
||||
dVecTilt(1) * vecTilt(2) - dVecTilt(2) * vecTilt(1));
|
||||
dVecTilt = dMatTiltdTauY * Vec3d(xd0, yd0, 1);
|
||||
dpdk_p[13] = fx * invProjSquare * (
|
||||
dVecTilt(0) * vecTilt(2) - dVecTilt(2) * vecTilt(0));
|
||||
dpdk_p[dpdk_step+13] = fy * invProjSquare * (
|
||||
dVecTilt(1) * vecTilt(2) - dVecTilt(2) * vecTilt(1));
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
dpdk_p += dpdk_step*2;
|
||||
}
|
||||
|
||||
if( dpdt_p )
|
||||
{
|
||||
double dxdt[] = { z, 0, -x*z }, dydt[] = { 0, z, -y*z };
|
||||
for( j = 0; j < 3; j++ )
|
||||
{
|
||||
double dr2dt = 2*x*dxdt[j] + 2*y*dydt[j];
|
||||
double dcdist_dt = k[0]*dr2dt + 2*k[1]*r2*dr2dt + 3*k[4]*r4*dr2dt;
|
||||
double dicdist2_dt = -icdist2*icdist2*(k[5]*dr2dt + 2*k[6]*r2*dr2dt + 3*k[7]*r4*dr2dt);
|
||||
double da1dt = 2*(x*dydt[j] + y*dxdt[j]);
|
||||
double dmxdt = (dxdt[j]*cdist*icdist2 + x*dcdist_dt*icdist2 + x*cdist*dicdist2_dt +
|
||||
k[2]*da1dt + k[3]*(dr2dt + 4*x*dxdt[j]) + k[8]*dr2dt + 2*r2*k[9]*dr2dt);
|
||||
double dmydt = (dydt[j]*cdist*icdist2 + y*dcdist_dt*icdist2 + y*cdist*dicdist2_dt +
|
||||
k[2]*(dr2dt + 4*y*dydt[j]) + k[3]*da1dt + k[10]*dr2dt + 2*r2*k[11]*dr2dt);
|
||||
dXdYd = dMatTilt*Vec2d(dmxdt, dmydt);
|
||||
dpdt_p[j] = fx*dXdYd(0);
|
||||
dpdt_p[dpdt_step+j] = fy*dXdYd(1);
|
||||
}
|
||||
dpdt_p += dpdt_step*2;
|
||||
}
|
||||
|
||||
if( dpdr_p )
|
||||
{
|
||||
double dx0dr[] =
|
||||
{
|
||||
X*dRdr[0] + Y*dRdr[1] + Z*dRdr[2],
|
||||
X*dRdr[9] + Y*dRdr[10] + Z*dRdr[11],
|
||||
X*dRdr[18] + Y*dRdr[19] + Z*dRdr[20]
|
||||
};
|
||||
double dy0dr[] =
|
||||
{
|
||||
X*dRdr[3] + Y*dRdr[4] + Z*dRdr[5],
|
||||
X*dRdr[12] + Y*dRdr[13] + Z*dRdr[14],
|
||||
X*dRdr[21] + Y*dRdr[22] + Z*dRdr[23]
|
||||
};
|
||||
double dz0dr[] =
|
||||
{
|
||||
X*dRdr[6] + Y*dRdr[7] + Z*dRdr[8],
|
||||
X*dRdr[15] + Y*dRdr[16] + Z*dRdr[17],
|
||||
X*dRdr[24] + Y*dRdr[25] + Z*dRdr[26]
|
||||
};
|
||||
for( j = 0; j < 3; j++ )
|
||||
{
|
||||
double dxdr = z*(dx0dr[j] - x*dz0dr[j]);
|
||||
double dydr = z*(dy0dr[j] - y*dz0dr[j]);
|
||||
double dr2dr = 2*x*dxdr + 2*y*dydr;
|
||||
double dcdist_dr = (k[0] + 2*k[1]*r2 + 3*k[4]*r4)*dr2dr;
|
||||
double dicdist2_dr = -icdist2*icdist2*(k[5] + 2*k[6]*r2 + 3*k[7]*r4)*dr2dr;
|
||||
double da1dr = 2*(x*dydr + y*dxdr);
|
||||
double dmxdr = (dxdr*cdist*icdist2 + x*dcdist_dr*icdist2 + x*cdist*dicdist2_dr +
|
||||
k[2]*da1dr + k[3]*(dr2dr + 4*x*dxdr) + (k[8] + 2*r2*k[9])*dr2dr);
|
||||
double dmydr = (dydr*cdist*icdist2 + y*dcdist_dr*icdist2 + y*cdist*dicdist2_dr +
|
||||
k[2]*(dr2dr + 4*y*dydr) + k[3]*da1dr + (k[10] + 2*r2*k[11])*dr2dr);
|
||||
dXdYd = dMatTilt*Vec2d(dmxdr, dmydr);
|
||||
dpdr_p[j] = fx*dXdYd(0);
|
||||
dpdr_p[dpdr_step+j] = fy*dXdYd(1);
|
||||
}
|
||||
dpdr_p += dpdr_step*2;
|
||||
}
|
||||
|
||||
if( dpdo_p )
|
||||
{
|
||||
double dxdo[] = { z * ( R[0] - x * z * z0 * R[6] ),
|
||||
z * ( R[1] - x * z * z0 * R[7] ),
|
||||
z * ( R[2] - x * z * z0 * R[8] ) };
|
||||
double dydo[] = { z * ( R[3] - y * z * z0 * R[6] ),
|
||||
z * ( R[4] - y * z * z0 * R[7] ),
|
||||
z * ( R[5] - y * z * z0 * R[8] ) };
|
||||
for( j = 0; j < 3; j++ )
|
||||
{
|
||||
double dr2do = 2 * x * dxdo[j] + 2 * y * dydo[j];
|
||||
double dr4do = 2 * r2 * dr2do;
|
||||
double dr6do = 3 * r4 * dr2do;
|
||||
double da1do = 2 * y * dxdo[j] + 2 * x * dydo[j];
|
||||
double da2do = dr2do + 4 * x * dxdo[j];
|
||||
double da3do = dr2do + 4 * y * dydo[j];
|
||||
double dcdist_do
|
||||
= k[0] * dr2do + k[1] * dr4do + k[4] * dr6do;
|
||||
double dicdist2_do = -icdist2 * icdist2
|
||||
* ( k[5] * dr2do + k[6] * dr4do + k[7] * dr6do );
|
||||
double dxd0_do = cdist * icdist2 * dxdo[j]
|
||||
+ x * icdist2 * dcdist_do + x * cdist * dicdist2_do
|
||||
+ k[2] * da1do + k[3] * da2do + k[8] * dr2do
|
||||
+ k[9] * dr4do;
|
||||
double dyd0_do = cdist * icdist2 * dydo[j]
|
||||
+ y * icdist2 * dcdist_do + y * cdist * dicdist2_do
|
||||
+ k[2] * da3do + k[3] * da1do + k[10] * dr2do
|
||||
+ k[11] * dr4do;
|
||||
dXdYd = dMatTilt * Vec2d( dxd0_do, dyd0_do );
|
||||
dpdo_p[i * 3 + j] = fx * dXdYd( 0 );
|
||||
dpdo_p[dpdo_step + i * 3 + j] = fy * dXdYd( 1 );
|
||||
}
|
||||
dpdo_p += dpdo_step * 2;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
if( _m != imagePoints )
|
||||
cvConvert( _m, imagePoints );
|
||||
|
||||
if( _dpdr != dpdr )
|
||||
cvConvert( _dpdr, dpdr );
|
||||
|
||||
if( _dpdt != dpdt )
|
||||
cvConvert( _dpdt, dpdt );
|
||||
|
||||
if( _dpdf != dpdf )
|
||||
cvConvert( _dpdf, dpdf );
|
||||
|
||||
if( _dpdc != dpdc )
|
||||
cvConvert( _dpdc, dpdc );
|
||||
|
||||
if( _dpdk != dpdk )
|
||||
cvConvert( _dpdk, dpdk );
|
||||
|
||||
if( _dpdo != dpdo )
|
||||
cvConvert( _dpdo, dpdo );
|
||||
}
|
||||
|
||||
static void _cvProjectPoints2( const CvMat* objectPoints,
|
||||
const CvMat* r_vec,
|
||||
const CvMat* t_vec,
|
||||
const CvMat* A,
|
||||
const CvMat* distCoeffs,
|
||||
CvMat* imagePoints, CvMat* dpdr,
|
||||
CvMat* dpdt, CvMat* dpdf,
|
||||
CvMat* dpdc, CvMat* dpdk,
|
||||
double aspectRatio )
|
||||
{
|
||||
_cvProjectPoints2Internal( objectPoints, r_vec, t_vec, A, distCoeffs, imagePoints, dpdr, dpdt,
|
||||
dpdf, dpdc, dpdk, NULL, aspectRatio );
|
||||
}
|
||||
|
||||
3
aruco_pose/vendor/VendorOpenCV.cmake
vendored
3
aruco_pose/vendor/VendorOpenCV.cmake
vendored
@@ -7,6 +7,7 @@ endif()
|
||||
|
||||
message(STATUS "Adding vendored aruco_pose OpenCV module")
|
||||
add_library(_opencv_aruco STATIC
|
||||
vendor/aruco/src/apriltag_quad_thresh.cpp
|
||||
vendor/aruco/src/aruco.cpp
|
||||
vendor/aruco/src/charuco.cpp
|
||||
vendor/aruco/src/dictionary.cpp
|
||||
@@ -23,7 +24,7 @@ target_compile_definitions(_opencv_aruco PRIVATE
|
||||
CV_OVERRIDE=override
|
||||
)
|
||||
target_compile_options(_opencv_aruco PRIVATE
|
||||
-fpic -fPIC
|
||||
-fpic -fPIC -fvisibility=hidden
|
||||
)
|
||||
|
||||
target_include_directories(_opencv_aruco PUBLIC
|
||||
|
||||
@@ -94,7 +94,7 @@ void ptsort_(struct pt *pts, int sz)
|
||||
|
||||
// Use stack storage if it's not too big.
|
||||
cv::AutoBuffer<struct pt, 1024> _tmp_stack(sz);
|
||||
memcpy(_tmp_stack.data(), pts, sizeof(struct pt) * sz);
|
||||
memcpy(_tmp_stack, pts, sizeof(struct pt) * sz);
|
||||
|
||||
int asz = sz/2;
|
||||
int bsz = sz - asz;
|
||||
@@ -470,11 +470,11 @@ int quad_segment_agg(int sz, struct line_fit_pt *lfps, int indices[4]){
|
||||
int rvalloc_pos = 0;
|
||||
int rvalloc_size = 3*sz;
|
||||
cv::AutoBuffer<struct remove_vertex, 0> rvalloc_(std::max(1, rvalloc_size));
|
||||
memset(rvalloc_.data(), 0, sizeof(rvalloc_[0]) * rvalloc_.size()); // TODO Add AutoBuffer zero fill
|
||||
struct remove_vertex *rvalloc = rvalloc_.data();
|
||||
memset(rvalloc_, 0, sizeof(rvalloc_[0]) * rvalloc_.size()); // TODO Add AutoBuffer zero fill
|
||||
struct remove_vertex *rvalloc = rvalloc_;
|
||||
cv::AutoBuffer<struct segment, 0> segs_(std::max(1, sz)); // TODO Add AutoBuffer zero fill
|
||||
memset(segs_.data(), 0, sizeof(segs_[0]) * segs_.size());
|
||||
struct segment *segs = segs_.data();
|
||||
memset(segs_, 0, sizeof(segs_[0]) * segs_.size());
|
||||
struct segment *segs = segs_;
|
||||
|
||||
// populate with initial entries
|
||||
for (int i = 0; i < sz; i++) {
|
||||
@@ -753,8 +753,8 @@ int fit_quad(const Ptr<DetectorParameters> &_params, const Mat im, zarray_t *clu
|
||||
// efficiently computed for any contiguous range of indices.
|
||||
|
||||
cv::AutoBuffer<struct line_fit_pt, 64> lfps_(sz);
|
||||
memset(lfps_.data(), 0, sizeof(lfps_[0]) * lfps_.size()); // TODO Add AutoBuffer zero fill
|
||||
struct line_fit_pt *lfps = lfps_.data();
|
||||
memset(lfps_, 0, sizeof(lfps_[0]) * lfps_.size()); // TODO Add AutoBuffer zero fill
|
||||
struct line_fit_pt *lfps = lfps_;
|
||||
|
||||
for (int i = 0; i < sz; i++) {
|
||||
struct pt *p;
|
||||
|
||||
@@ -1,4 +1,4 @@
|
||||
#!/usr/bin/env python
|
||||
#!/usr/bin/env python3
|
||||
|
||||
from distutils.core import setup
|
||||
|
||||
|
||||
18
builder/assets/noetic-rosdep-clover.yaml
Normal file
18
builder/assets/noetic-rosdep-clover.yaml
Normal file
@@ -0,0 +1,18 @@
|
||||
async_web_server_cpp:
|
||||
debian:
|
||||
buster: [ros-noetic-async-web-server-cpp]
|
||||
led_msgs:
|
||||
debian:
|
||||
buster: [ros-noetic-led-msgs]
|
||||
ros_pytest:
|
||||
debian:
|
||||
buster: [ros-noetic-ros-pytest]
|
||||
tf2_web_republisher:
|
||||
debian:
|
||||
buster: [ros-noetic-tf2-web-republisher]
|
||||
web_video_server:
|
||||
debian:
|
||||
buster: [ros-noetic-web-video-server]
|
||||
ws281x:
|
||||
debian:
|
||||
buster: [ros-noetic-ws281x]
|
||||
@@ -3,7 +3,7 @@ Description=Launcher for the ROS master, parameter server and rosout logging nod
|
||||
|
||||
[Service]
|
||||
User=pi
|
||||
ExecStart=/bin/sh -c ". /opt/ros/melodic/setup.sh; ROS_HOSTNAME=`hostname`.local exec roscore"
|
||||
ExecStart=/bin/sh -c ". /opt/ros/noetic/setup.sh; ROS_HOSTNAME=`hostname`.local exec roscore"
|
||||
Restart=on-failure
|
||||
RestartSec=3
|
||||
|
||||
|
||||
@@ -117,7 +117,7 @@ ${BUILDER_DIR}/image-chroot.sh ${IMAGE_PATH} copy ${SCRIPTS_DIR}'/assets/avahi-s
|
||||
# Clover
|
||||
${BUILDER_DIR}/image-chroot.sh ${IMAGE_PATH} copy ${SCRIPTS_DIR}'/assets/clover.service' '/lib/systemd/system/'
|
||||
${BUILDER_DIR}/image-chroot.sh ${IMAGE_PATH} copy ${SCRIPTS_DIR}'/assets/roscore.service' '/lib/systemd/system/'
|
||||
${BUILDER_DIR}/image-chroot.sh ${IMAGE_PATH} copy ${SCRIPTS_DIR}'/assets/melodic-rosdep-clover.yaml' '/etc/ros/rosdep/'
|
||||
${BUILDER_DIR}/image-chroot.sh ${IMAGE_PATH} copy ${SCRIPTS_DIR}'/assets/noetic-rosdep-clover.yaml' '/etc/ros/rosdep/'
|
||||
${BUILDER_DIR}/image-chroot.sh ${IMAGE_PATH} copy ${SCRIPTS_DIR}'/assets/ros_python_paths' '/etc/sudoers.d/'
|
||||
${BUILDER_DIR}/image-chroot.sh ${IMAGE_PATH} copy ${SCRIPTS_DIR}'/assets/pigpiod.service' '/lib/systemd/system/'
|
||||
${BUILDER_DIR}/image-chroot.sh ${IMAGE_PATH} copy ${SCRIPTS_DIR}'/assets/launch.nanorc' '/usr/share/nano/'
|
||||
|
||||
@@ -21,6 +21,9 @@ INSTALL_ROS_PACK_SOURCES=$3
|
||||
DISCOVER_ROS_PACK=$4
|
||||
NUMBER_THREADS=$5
|
||||
|
||||
# Current ROS distribution
|
||||
ROS_DISTRO=noetic
|
||||
|
||||
echo_stamp() {
|
||||
# TEMPLATE: echo_stamp <TEXT> <TYPE>
|
||||
# TYPE: SUCCESS, ERROR, INFO
|
||||
@@ -68,7 +71,8 @@ my_travis_retry() {
|
||||
# TODO: 'kinetic-rosdep-clover.yaml' should add only if we use our repo?
|
||||
echo_stamp "Init rosdep"
|
||||
my_travis_retry rosdep init
|
||||
echo "yaml file:///etc/ros/rosdep/melodic-rosdep-clover.yaml" >> /etc/ros/rosdep/sources.list.d/20-default.list
|
||||
# FIXME: Re-add this after missing packages are built
|
||||
echo "yaml file:///etc/ros/rosdep/${ROS_DISTRO}-rosdep-clover.yaml" >> /etc/ros/rosdep/sources.list.d/20-default.list
|
||||
my_travis_retry rosdep update
|
||||
|
||||
echo_stamp "Populate rosdep for ROS user"
|
||||
@@ -76,22 +80,39 @@ my_travis_retry sudo -u pi rosdep update
|
||||
|
||||
export ROS_IP='127.0.0.1' # needed for running tests
|
||||
|
||||
# echo_stamp "Reconfiguring Clover repository for simplier unshallowing" # TODO: bring back
|
||||
# cd /home/pi/catkin_ws/src/clover
|
||||
# git config remote.origin.fetch "+refs/heads/*:refs/remotes/origin/*"
|
||||
echo_stamp "Remove .git from Clover to reduce the size"
|
||||
rm -rf /home/pi/catkin_ws/src/clover/.git # TODO: remove
|
||||
# echo_stamp "Reconfiguring Clover repository for simplier unshallowing"
|
||||
cd /home/pi/catkin_ws/src/clover
|
||||
git config remote.origin.fetch "+refs/heads/*:refs/remotes/origin/*"
|
||||
|
||||
# This is sort of a hack to force "custom" packages to be installed - the ones built by COEX, linked against OpenCV 4.2
|
||||
# I **wish** OpenCV would not be such a mess, but, well, here we are.
|
||||
echo_stamp "Installing OpenCV 4.2-compatible ROS packages"
|
||||
apt install -y --no-install-recommends \
|
||||
ros-${ROS_DISTRO}-compressed-image-transport=1.14.0-0buster \
|
||||
ros-${ROS_DISTRO}-cv-bridge=1.15.0-0buster \
|
||||
ros-${ROS_DISTRO}-cv-camera=0.5.0-0buster \
|
||||
ros-${ROS_DISTRO}-image-publisher=1.15.3-0buster \
|
||||
ros-${ROS_DISTRO}-web-video-server=0.2.1-0buster
|
||||
apt-mark hold \
|
||||
ros-${ROS_DISTRO}-compressed-image-transport \
|
||||
ros-${ROS_DISTRO}-cv-bridge \
|
||||
ros-${ROS_DISTRO}-cv-camera \
|
||||
ros-${ROS_DISTRO}-image-publisher \
|
||||
ros-${ROS_DISTRO}-web-video-server
|
||||
|
||||
echo_stamp "Installing libboost-dev" # https://travis-ci.org/github/CopterExpress/clover/jobs/766318908#L6536
|
||||
my_travis_retry apt-get install -y --no-install-recommends libboost-dev libboost-all-dev
|
||||
|
||||
echo_stamp "Build and install Clover"
|
||||
cd /home/pi/catkin_ws
|
||||
# Don't try to install gazebo_ros
|
||||
my_travis_retry rosdep install -y --from-paths src --ignore-src --rosdistro melodic --os=debian:buster \
|
||||
my_travis_retry rosdep install -y --from-paths src --ignore-src --rosdistro ${ROS_DISTRO} --os=debian:buster \
|
||||
--skip-keys=gazebo_ros --skip-keys=gazebo_plugins
|
||||
my_travis_retry pip install wheel
|
||||
my_travis_retry pip install -r /home/pi/catkin_ws/src/clover/clover/requirements.txt
|
||||
source /opt/ros/melodic/setup.bash
|
||||
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=Release -DCATKIN_BLACKLIST_PACKAGES=clover_gazebo_plugins
|
||||
catkin_make -j2 -DCMAKE_BUILD_TYPE=RelWithDebInfo -DCATKIN_BLACKLIST_PACKAGES=clover_gazebo_plugins
|
||||
|
||||
echo_stamp "Install clever package (for backwards compatibility)"
|
||||
cd /home/pi/catkin_ws/src/clover/builder/assets/clever
|
||||
@@ -108,23 +129,20 @@ touch node_modules/CATKIN_IGNORE docs/CATKIN_IGNORE _book/CATKIN_IGNORE clover/w
|
||||
|
||||
echo_stamp "Installing additional ROS packages"
|
||||
my_travis_retry apt-get install -y --no-install-recommends \
|
||||
ros-melodic-dynamic-reconfigure \
|
||||
ros-melodic-compressed-image-transport \
|
||||
ros-melodic-rosbridge-suite \
|
||||
ros-melodic-rosserial \
|
||||
ros-melodic-usb-cam \
|
||||
ros-melodic-vl53l1x \
|
||||
ros-melodic-ws281x \
|
||||
ros-melodic-rosshow
|
||||
ros-${ROS_DISTRO}-dynamic-reconfigure \
|
||||
ros-${ROS_DISTRO}-rosbridge-suite \
|
||||
ros-${ROS_DISTRO}-rosserial \
|
||||
ros-${ROS_DISTRO}-usb-cam \
|
||||
ros-${ROS_DISTRO}-vl53l1x \
|
||||
ros-${ROS_DISTRO}-ws281x \
|
||||
ros-${ROS_DISTRO}-rosshow \
|
||||
ros-${ROS_DISTRO}-cmake-modules \
|
||||
ros-${ROS_DISTRO}-image-view
|
||||
|
||||
# TODO move GeographicLib datasets to Mavros debian package
|
||||
echo_stamp "Install GeographicLib datasets (needed for mavros)" \
|
||||
&& wget -qO- https://raw.githubusercontent.com/mavlink/mavros/master/mavros/scripts/install_geographiclib_datasets.sh | bash
|
||||
|
||||
# FIXME: Buster comes with tornado==5.1.1 but we need tornado==4.2.1 for rosbridge_suite
|
||||
# (note that Python 3 will still have a more recent version)
|
||||
pip install tornado==4.2.1
|
||||
|
||||
echo_stamp "Running tests"
|
||||
cd /home/pi/catkin_ws
|
||||
# FIXME: Investigate failing tests
|
||||
@@ -141,7 +159,7 @@ cat << EOF >> /home/pi/.bashrc
|
||||
LANG='C.UTF-8'
|
||||
LC_ALL='C.UTF-8'
|
||||
export ROS_HOSTNAME=\`hostname\`.local
|
||||
source /opt/ros/melodic/setup.bash
|
||||
source /opt/ros/${ROS_DISTRO}/setup.bash
|
||||
source /home/pi/catkin_ws/devel/setup.bash
|
||||
EOF
|
||||
|
||||
|
||||
@@ -64,15 +64,14 @@ echo "APT::Acquire::Retries \"3\";" > /etc/apt/apt.conf.d/80-retries
|
||||
echo_stamp "Install apt keys & repos"
|
||||
|
||||
# TODO: This STDOUT consist 'OK'
|
||||
curl http://deb.coex.tech/aptly_repo_signing.key 2> /dev/null | apt-key add -
|
||||
apt-get update \
|
||||
&& apt-get install --no-install-recommends -y dirmngr > /dev/null \
|
||||
&& apt-key adv --keyserver hkp://keyserver.ubuntu.com:80 --recv-key C1CF6E31E6BADE8868B172B4F42ED6FBAB17C654
|
||||
|
||||
echo "deb http://packages.ros.org/ros/ubuntu buster main" > /etc/apt/sources.list.d/ros-latest.list
|
||||
echo "deb http://deb.coex.tech/opencv3 buster main" > /etc/apt/sources.list.d/opencv3.list
|
||||
echo "deb http://deb.coex.tech/rpi-ros-melodic buster main" > /etc/apt/sources.list.d/rpi-ros-melodic.list
|
||||
echo "deb http://deb.coex.tech/clover buster main" > /etc/apt/sources.list.d/clover.list
|
||||
|
||||
wget -O - 'http://packages.coex.tech/key.asc' | apt-key add -
|
||||
echo 'deb http://packages.coex.tech buster main' >> /etc/apt/sources.list
|
||||
|
||||
echo_stamp "Update apt cache"
|
||||
|
||||
@@ -99,18 +98,18 @@ tree \
|
||||
vim \
|
||||
libjpeg8 \
|
||||
tcpdump \
|
||||
ltrace \
|
||||
libpoco-dev \
|
||||
libzbar0 \
|
||||
python-rosdep \
|
||||
python-rosinstall-generator \
|
||||
python-wstool \
|
||||
python-rosinstall \
|
||||
python3-rosdep \
|
||||
python3-rosinstall-generator \
|
||||
python3-wstool \
|
||||
python3-rosinstall \
|
||||
build-essential \
|
||||
libffi-dev \
|
||||
monkey \
|
||||
pigpio python-pigpio python3-pigpio \
|
||||
i2c-tools \
|
||||
espeak espeak-data python-espeak python3-espeak \
|
||||
ntpdate \
|
||||
python-dev \
|
||||
python3-dev \
|
||||
@@ -144,7 +143,7 @@ my_travis_retry pip3 install butterfly[systemd]
|
||||
systemctl enable butterfly.socket
|
||||
|
||||
echo_stamp "Install ws281x library"
|
||||
my_travis_retry pip install --prefer-binary rpi_ws281x
|
||||
my_travis_retry pip3 install --prefer-binary rpi_ws281x
|
||||
|
||||
echo_stamp "Setup Monkey"
|
||||
mv /etc/monkey/sites/default /etc/monkey/sites/default.orig
|
||||
|
||||
@@ -16,16 +16,20 @@ set -ex
|
||||
|
||||
echo "Run image tests"
|
||||
|
||||
export ROS_DISTRO='melodic'
|
||||
export ROS_DISTRO='noetic'
|
||||
export ROS_IP='127.0.0.1'
|
||||
source /opt/ros/melodic/setup.bash
|
||||
source /opt/ros/${ROS_DISTRO}/setup.bash
|
||||
source /home/pi/catkin_ws/devel/setup.bash
|
||||
systemctl start roscore
|
||||
|
||||
cd /home/pi/catkin_ws/src/clover/builder/test/
|
||||
./tests.sh
|
||||
./tests.py
|
||||
./tests_py3.py
|
||||
[[ $(./test_qr.py) == "Found QRCODE with data Проверка Unicode with center at x=66.0, y=66.0" ]]
|
||||
[[ $(./tests_clever.py) == "Warning: clever package is renamed to clover" ]] # test backwards compatibility
|
||||
|
||||
systemctl stop roscore
|
||||
|
||||
echo "Move /etc/ld.so.preload back to its original position"
|
||||
mv /etc/ld.so.preload.disabled-for-build /etc/ld.so.preload
|
||||
|
||||
@@ -6,21 +6,40 @@ set -e
|
||||
apt-key adv --keyserver 'hkp://keyserver.ubuntu.com:80' --recv-key C1CF6E31E6BADE8868B172B4F42ED6FBAB17C654 # https://github.com/osrf/docker_images/issues/535
|
||||
apt-get update
|
||||
apt-get install -y curl
|
||||
curl https://bootstrap.pypa.io/pip/2.7/get-pip.py -o get-pip.py
|
||||
python ./get-pip.py
|
||||
if [ "x${ROS_PYTHON_VERSION}" = "x3" ]; then
|
||||
PYTHON=python3
|
||||
curl https://bootstrap.pypa.io/get-pip.py -o get-pip.py
|
||||
else
|
||||
PYTHON=python
|
||||
curl https://bootstrap.pypa.io/pip/2.7/get-pip.py -o get-pip.py
|
||||
fi
|
||||
${PYTHON} ./get-pip.py
|
||||
|
||||
# Step 1.5: Add deb.coex.tech to apt
|
||||
curl http://deb.coex.tech/aptly_repo_signing.key 2> /dev/null | apt-key add -
|
||||
echo "deb http://deb.coex.tech/ros xenial main" > /etc/apt/sources.list.d/coex.tech.list
|
||||
echo "yaml file:///etc/ros/rosdep/coex.yaml" > /etc/ros/rosdep/sources.list.d/99-coex.list
|
||||
CODENAME=$(lsb_release -sc)
|
||||
|
||||
cat <<EOF > /etc/ros/rosdep/coex.yaml
|
||||
led_msgs:
|
||||
ubuntu:
|
||||
xenial: ros-kinetic-led-msgs
|
||||
bionic: ros-melodic-led-msgs
|
||||
debian:
|
||||
stretch: ros-kinetic-led-msgs
|
||||
buster: ros-melodic-led-msgs
|
||||
${CODENAME}: [ros-${ROS_DISTRO}-led-msgs]
|
||||
async_web_server_cpp:
|
||||
ubuntu:
|
||||
${CODENAME}: [ros-${ROS_DISTRO}-async-web-server-cpp]
|
||||
ros_pytest:
|
||||
ubuntu:
|
||||
${CODENAME}: [ros-${ROS_DISTRO}-ros-pytest]
|
||||
tf2_web_republisher:
|
||||
ubuntu:
|
||||
${CODENAME}: [ros-${ROS_DISTRO}-tf2-web-republisher]
|
||||
web_video_server:
|
||||
ubuntu:
|
||||
${CODENAME}: [ros-${ROS_DISTRO}-web-video-server]
|
||||
ws281x:
|
||||
ubuntu:
|
||||
${CODENAME}: [ros-${ROS_DISTRO}-ws281x]
|
||||
EOF
|
||||
apt-get update
|
||||
rosdep update
|
||||
@@ -38,7 +57,7 @@ cd /root/catkin_ws
|
||||
catkin_make
|
||||
|
||||
# Step 4: Run tests
|
||||
pip install --upgrade pytest
|
||||
${PYTHON} -m pip install --upgrade pytest
|
||||
cd /root/catkin_ws
|
||||
source devel/setup.bash
|
||||
catkin_make run_tests && catkin_test_results
|
||||
|
||||
BIN
builder/test/qr.png
Normal file
BIN
builder/test/qr.png
Normal file
Binary file not shown.
|
After Width: | Height: | Size: 1.8 KiB |
42
builder/test/test_qr.py
Executable file
42
builder/test/test_qr.py
Executable file
@@ -0,0 +1,42 @@
|
||||
#!/usr/bin/env python3
|
||||
|
||||
# Test QG recognition example
|
||||
# Should be synced with the documentation: /docs/en/camera.md, /docs/ru/camera.md
|
||||
# TODO: use real ROS topics
|
||||
|
||||
import rospy
|
||||
from pyzbar import pyzbar
|
||||
from cv_bridge import CvBridge
|
||||
from sensor_msgs.msg import Image
|
||||
|
||||
bridge = CvBridge()
|
||||
|
||||
# rospy.init_node('barcode_test')
|
||||
|
||||
# Image subscriber callback function
|
||||
def image_callback(data):
|
||||
cv_image = bridge.imgmsg_to_cv2(data, 'bgr8') # OpenCV image
|
||||
barcodes = pyzbar.decode(cv_image)
|
||||
for barcode in barcodes:
|
||||
b_data = barcode.data.decode("utf-8")
|
||||
b_type = barcode.type
|
||||
(x, y, w, h) = barcode.rect
|
||||
xc = x + w/2
|
||||
yc = y + h/2
|
||||
print("Found {} with data {} with center at x={}, y={}".format(b_type, b_data, xc, yc))
|
||||
# rospy.signal_shutdown('done')
|
||||
|
||||
# image_sub = rospy.Subscriber('main_camera/image_raw', Image, image_callback, queue_size=1)
|
||||
|
||||
# ==============================================================================
|
||||
# Publish test image
|
||||
# rospy.sleep(2)
|
||||
|
||||
import cv2
|
||||
img = cv2.imread('qr.png')
|
||||
image_callback(bridge.cv2_to_imgmsg(img, 'bgr8'))
|
||||
|
||||
# image_pub = rospy.Publisher('/main_camera/image_raw', Image, queue_size=1, latch=True)
|
||||
# image_pub.publish(bridge.cv2_to_imgmsg(img, 'bgr8'))
|
||||
|
||||
# rospy.spin()
|
||||
@@ -1,21 +1,29 @@
|
||||
#!/usr/bin/env python
|
||||
#!/usr/bin/env python3
|
||||
|
||||
# validate all required modules installed
|
||||
|
||||
import rospy
|
||||
from geometry_msgs.msg import PoseStamped
|
||||
from sensor_msgs.msg import Range, BatteryState
|
||||
|
||||
import cv2
|
||||
import cv2.aruco
|
||||
from sensor_msgs.msg import Image
|
||||
from cv_bridge import CvBridge
|
||||
|
||||
import numpy
|
||||
import mavros
|
||||
from mavros_msgs.msg import State, StatusText, ExtendedState
|
||||
from mavros_msgs.msg import State, StatusText, ExtendedState, RCIn, Mavlink
|
||||
from mavros_msgs.srv import CommandBool, CommandLong, SetMode
|
||||
|
||||
from std_srvs.srv import Trigger
|
||||
from clover.srv import GetTelemetry, Navigate, NavigateGlobal, SetPosition, SetVelocity, \
|
||||
SetAttitude, SetRates, SetLEDEffect
|
||||
from led_msgs.srv import SetLEDs
|
||||
from led_msgs.msg import LEDStateArray, LEDState
|
||||
from aruco_pose.msg import Marker, MarkerArray, Point2D
|
||||
|
||||
import dynamic_reconfigure.client
|
||||
|
||||
import tf2_ros
|
||||
import tf2_geometry_msgs
|
||||
@@ -28,4 +36,4 @@ import pigpio
|
||||
# from espeak import espeak
|
||||
from pyzbar import pyzbar
|
||||
|
||||
print cv2.getBuildInformation()
|
||||
print(cv2.getBuildInformation())
|
||||
|
||||
@@ -54,6 +54,8 @@ rosversion usb_cam
|
||||
rosversion cv_camera
|
||||
rosversion web_video_server
|
||||
rosversion rosshow
|
||||
rosversion nodelet
|
||||
rosversion image_view
|
||||
|
||||
# validate examples are present
|
||||
[[ $(ls /home/pi/examples/*) ]]
|
||||
|
||||
@@ -1,4 +1,4 @@
|
||||
#!/usr/bin/env python
|
||||
#!/usr/bin/env python3
|
||||
|
||||
# test backwards compatibility
|
||||
|
||||
|
||||
@@ -1,4 +1,4 @@
|
||||
cmake_minimum_required(VERSION 2.8.3)
|
||||
cmake_minimum_required(VERSION 3.0)
|
||||
project(clover)
|
||||
|
||||
## Compile as C++11, supported in ROS Kinetic and newer
|
||||
@@ -30,7 +30,15 @@ list(APPEND CMAKE_MODULE_PATH "${CMAKE_CURRENT_LIST_DIR}/cmake")
|
||||
|
||||
find_package(GeographicLib REQUIRED)
|
||||
|
||||
find_package(OpenCV 3 REQUIRED
|
||||
# Workaround for OpenCV 3/4 support
|
||||
set(_opencv_version 4)
|
||||
find_package(OpenCV ${_opencv_version} QUIET COMPONENTS calib3d imgproc)
|
||||
if (NOT OpenCV_FOUND)
|
||||
message(STATUS "Did not find OpenCV 4, searching for OpenCV 3")
|
||||
set(_opencv_version 3)
|
||||
endif()
|
||||
|
||||
find_package(OpenCV ${_opencv_version} REQUIRED
|
||||
COMPONENTS
|
||||
calib3d
|
||||
imgproc
|
||||
@@ -254,6 +262,10 @@ target_link_libraries(${PROJECT_NAME}
|
||||
# DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
|
||||
# )
|
||||
|
||||
catkin_install_python(PROGRAMS src/selfcheck.py
|
||||
DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
|
||||
)
|
||||
|
||||
# Only install udev rules when building a Debian package
|
||||
# FIXME: Other operating systems may have other prefixes
|
||||
string(FIND ${CMAKE_INSTALL_PREFIX} "/opt/ros" _PREFIX_INDEX)
|
||||
|
||||
@@ -4,7 +4,7 @@ A bundle for autonomous navigation and drone control.
|
||||
|
||||
## Manual installation
|
||||
|
||||
Install ROS Melodic according to the [documentation](http://wiki.ros.org/melodic/Installation), then [create a Catkin workspace](http://wiki.ros.org/catkin/Tutorials/create_a_workspace).
|
||||
Install ROS Noetic according to the [documentation](http://wiki.ros.org/noetic/Installation), then [create a Catkin workspace](http://wiki.ros.org/catkin/Tutorials/create_a_workspace).
|
||||
|
||||
Clone this repo to directory `~/catkin_ws/src/clover`:
|
||||
|
||||
|
||||
@@ -2,30 +2,37 @@
|
||||
<arg name="aruco_detect" default="true"/>
|
||||
<arg name="aruco_map" default="false"/>
|
||||
<arg name="aruco_vpe" default="false"/>
|
||||
<arg name="placement" default="floor"/> <!-- markers placement: floor, ceiling, unknown -->
|
||||
<arg name="length" default="0.33"/> <!-- not-in-map markers length, m -->
|
||||
<arg name="map" default="map.txt"/> <!-- markers map file name -->
|
||||
|
||||
<!-- For additional help go to https://clover.coex.tech/aruco -->
|
||||
|
||||
<!-- aruco_detect: detect aruco markers, estimate poses -->
|
||||
<node name="aruco_detect" pkg="nodelet" if="$(arg aruco_detect)" type="nodelet" args="load aruco_pose/aruco_detect nodelet_manager" output="screen" clear_params="true">
|
||||
<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/markers" if="$(arg aruco_map)"/>
|
||||
<param name="estimate_poses" value="true"/>
|
||||
<param name="send_tf" value="true"/>
|
||||
<param name="known_tilt" value="map"/>
|
||||
<param name="length" value="0.33"/>
|
||||
<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)"/>
|
||||
<!-- aruco detector parameters -->
|
||||
<param name="cornerRefinementMethod" value="2"/> <!-- contour refinement -->
|
||||
<param name="minMarkerPerimeterRate" value="0.075"/> <!-- 0.075 for 320x240, 0.0375 for 640x480 -->
|
||||
<!-- length override example: -->
|
||||
<!-- <param name="length_override/3" value="0.1"/> -->
|
||||
</node>
|
||||
|
||||
<!-- aruco_map: estimate aruco map pose -->
|
||||
<node name="aruco_map" pkg="nodelet" type="nodelet" if="$(arg aruco_map)" args="load aruco_pose/aruco_map nodelet_manager" output="screen" clear_params="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/map.txt"/>
|
||||
<param name="known_tilt" value="map"/>
|
||||
<param name="map" value="$(find aruco_pose)/map/$(arg map)"/>
|
||||
<param name="known_tilt" value="map" if="$(eval placement == 'floor')"/>
|
||||
<param name="known_tilt" value="map_flipped" if="$(eval placement == 'ceiling')"/>
|
||||
<param name="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)"/>
|
||||
|
||||
@@ -36,18 +36,13 @@
|
||||
<include file="$(find clover)/launch/aruco.launch" if="$(arg aruco)"/>
|
||||
|
||||
<!-- optical flow -->
|
||||
<node pkg="nodelet" type="nodelet" name="optical_flow" args="load clover/optical_flow nodelet_manager" if="$(arg optical_flow)" clear_params="true" output="screen">
|
||||
<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">
|
||||
<remap from="image_raw" to="main_camera/image_raw"/>
|
||||
<remap from="camera_info" to="main_camera/camera_info"/>
|
||||
<param name="calc_flow_gyro" value="true"/>
|
||||
<param name="roi_rad" value="0.8"/>
|
||||
</node>
|
||||
|
||||
<!-- main nodelet manager -->
|
||||
<node pkg="nodelet" type="nodelet" name="nodelet_manager" args="manager" output="screen" clear_params="true">
|
||||
<param name="num_worker_threads" value="2"/>
|
||||
</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 -->
|
||||
|
||||
@@ -3,6 +3,7 @@
|
||||
|
||||
<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="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"/>
|
||||
@@ -17,9 +18,14 @@
|
||||
<!-- static_transform_publisher arguments: x y z yaw pitch roll frame_id child_frame_id -->
|
||||
<!-- <node pkg="tf2_ros" type="static_transform_publisher" name="main_camera_frame" args="0.05 0 -0.07 -1.5707963 0 3.1415926 base_link main_camera_optical"/> -->
|
||||
|
||||
<!-- camera nodelet manager -->
|
||||
<node pkg="nodelet" type="nodelet" name="main_camera_nodelet_manager" args="manager" output="screen" clear_params="true" respawn="true">
|
||||
<param name="num_worker_threads" value="2"/>
|
||||
</node>
|
||||
|
||||
<!-- camera node -->
|
||||
<node pkg="nodelet" type="nodelet" name="main_camera" args="load cv_camera/CvCameraNodelet nodelet_manager" clear_params="true" unless="$(arg simulator)">
|
||||
<param name="device_path" value="/dev/video0"/> <!-- v4l2 device -->
|
||||
<node pkg="nodelet" type="nodelet" name="main_camera" args="load cv_camera/CvCameraNodelet main_camera_nodelet_manager" launch-prefix="rosrun clover waitfile $(arg device)" clear_params="true" unless="$(arg simulator)" respawn="true">
|
||||
<param name="device_path" value="$(arg device)"/>
|
||||
<param name="frame_id" value="main_camera_optical"/>
|
||||
<param name="camera_info_url" value="file://$(find clover)/camera_info/fisheye_cam.yaml"/>
|
||||
|
||||
|
||||
@@ -1,5 +1,5 @@
|
||||
<?xml version="1.0"?>
|
||||
<package format="2">
|
||||
<package format="3">
|
||||
<name>clover</name>
|
||||
<version>0.21.1</version>
|
||||
<description>The Clover package</description>
|
||||
@@ -37,7 +37,8 @@
|
||||
<depend>rosbridge_server</depend>
|
||||
<depend>web_video_server</depend>
|
||||
<depend>tf2_web_republisher</depend>
|
||||
<depend>python-lxml</depend>
|
||||
<depend condition="$ROS_PYTHON_VERSION == 2">python-lxml</depend>
|
||||
<depend condition="$ROS_PYTHON_VERSION == 3">python3-lxml</depend>
|
||||
<exec_depend>python-pymavlink</exec_depend>
|
||||
<!-- Use test_depend for packages you need only for testing: -->
|
||||
<!-- <test_depend>gtest</test_depend> -->
|
||||
|
||||
@@ -138,7 +138,7 @@ def mavlink_exec(cmd, timeout=3.0):
|
||||
timeout=3,
|
||||
baudrate=0,
|
||||
count=len(cmd),
|
||||
data=map(ord, cmd.ljust(70, '\0')))
|
||||
data=[ord(c) for c in cmd.ljust(70, '\0')])
|
||||
msg.pack(link)
|
||||
ros_msg = mavlink.convert_to_rosmsg(msg)
|
||||
mavlink_pub.publish(ros_msg)
|
||||
@@ -609,7 +609,7 @@ def check_rangefinder():
|
||||
|
||||
@check('Boot duration')
|
||||
def check_boot_duration():
|
||||
output = subprocess.check_output('systemd-analyze')
|
||||
output = subprocess.check_output('systemd-analyze').decode()
|
||||
r = re.compile(r'([\d\.]+)s\s*$', flags=re.MULTILINE)
|
||||
duration = float(r.search(output).groups()[0])
|
||||
if duration > 15:
|
||||
@@ -620,7 +620,7 @@ def check_boot_duration():
|
||||
def check_cpu_usage():
|
||||
WHITELIST = 'nodelet',
|
||||
CMD = "top -n 1 -b -i | tail -n +8 | awk '{ printf(\"%-8s\\t%-8s\\t%-8s\\n\", $1, $9, $12); }'"
|
||||
output = subprocess.check_output(CMD, shell=True)
|
||||
output = subprocess.check_output(CMD, shell=True).decode()
|
||||
processes = output.split('\n')
|
||||
for process in processes:
|
||||
if not process:
|
||||
@@ -636,7 +636,7 @@ def check_cpu_usage():
|
||||
def check_clover_service():
|
||||
try:
|
||||
output = subprocess.check_output('systemctl show -p ActiveState --value clover.service'.split(),
|
||||
stderr=subprocess.STDOUT)
|
||||
stderr=subprocess.STDOUT).decode()
|
||||
except subprocess.CalledProcessError as e:
|
||||
failure('systemctl returned %s: %s', e.returncode, e.output)
|
||||
return
|
||||
@@ -751,7 +751,7 @@ def check_rpi_health():
|
||||
# <parameter>=<value>
|
||||
# In case of `get_throttled`, <value> is a hexadecimal number
|
||||
# with some of the FLAGs OR'ed together
|
||||
output = subprocess.check_output(['vcgencmd', 'get_throttled'])
|
||||
output = subprocess.check_output(['vcgencmd', 'get_throttled']).decode()
|
||||
except OSError:
|
||||
failure('could not call vcgencmd binary; not a Raspberry Pi?')
|
||||
return
|
||||
|
||||
@@ -26,5 +26,10 @@ def test_simple_offboard_services_available():
|
||||
rospy.wait_for_service('land', timeout=5)
|
||||
|
||||
def test_web_video_server(node):
|
||||
import urllib2
|
||||
urllib2.urlopen("http://localhost:8080").read()
|
||||
try:
|
||||
# Python 2
|
||||
import urllib2 as urllib
|
||||
except ModuleNotFoundError:
|
||||
# Python 3
|
||||
import urllib.request as urllib
|
||||
urllib.urlopen("http://localhost:8080").read()
|
||||
|
||||
@@ -112,7 +112,7 @@ def run(req):
|
||||
'print': _print,
|
||||
'raw_input': _input}
|
||||
try:
|
||||
exec req.code in g
|
||||
exec(req.code, g)
|
||||
except Stop:
|
||||
rospy.loginfo('Program forced to stop')
|
||||
except Exception as e:
|
||||
|
||||
@@ -391,7 +391,7 @@ Blockly.Python.set_led = function(block) {
|
||||
|
||||
if (/^'(.*)'$/.test(colorCode)) { // is simple string
|
||||
let color = parseColor(colorCode);
|
||||
return `set_leds([LEDState(index=${index}, r=${color.r}, g=${color.g}, b=${color.b})])\n`;
|
||||
return `set_leds([LEDState(index=int(${index}), r=${color.r}, g=${color.g}, b=${color.b})])\n`; // TODO: check for simple int
|
||||
} else {
|
||||
let parseColor = Blockly.Python.provideFunction_('parse_color', [PARSE_COLOR]);
|
||||
return `set_leds([LEDState(index=${index}, **${parseColor}(${colorCode}))])\n`;
|
||||
|
||||
@@ -88,6 +88,6 @@ def aruco_gen():
|
||||
off_x + marker.x, off_y + marker.y, off_z + marker.z,
|
||||
marker.roll, marker.pitch, marker.yaw)
|
||||
|
||||
output = open(source_world, 'w') if inplace else stdout
|
||||
output = open(source_world, 'wb') if inplace else stdout
|
||||
|
||||
save_world(world_tree, output)
|
||||
|
||||
BIN
docs/assets/noetic.png
Normal file
BIN
docs/assets/noetic.png
Normal file
Binary file not shown.
|
After Width: | Height: | Size: 48 KiB |
@@ -91,7 +91,9 @@
|
||||
* [Soldering safety](tb.md)
|
||||
* [LED strip (legacy)](leds_old.md)
|
||||
* [Contribution Guidelines](contributing.md)
|
||||
* [COEX packages repository](packages.md)
|
||||
* [Migration to v0.20](migrate20.md)
|
||||
* [Migration to v0.22](migrate22.md)
|
||||
* [Events](events.md)
|
||||
* [CopterHack-2022](copterhack2022.md)
|
||||
* [CopterHack-2021](copterhack2021.md)
|
||||
|
||||
@@ -1,5 +1,9 @@
|
||||
# Map-based navigation with ArUco markers
|
||||
|
||||
> **Note** The following applies to [image versions](image.md) **0.22** and up. Older documentation is still available for [for version **0.20**](https://github.com/CopterExpress/clover/blob/v0.20/docs/en/aruco_map.md).
|
||||
|
||||
<!-- -->
|
||||
|
||||
> **Info** Marker detection requires the camera module to be correctly plugged in and [configured](camera_setup.md).
|
||||
|
||||
<!-- -->
|
||||
@@ -39,13 +43,14 @@ marker_id marker_size x y z z_angle y_angle x_angle
|
||||
|
||||
`N_angle` is the angle of rotation along the `N` axis in radians.
|
||||
|
||||
Map path is defined in the `map` parameter:
|
||||
Файлы карт располагаются в каталоге `~/catkin_ws/src/clover/aruco_pose/map`. Название файла с картой задается в аргументе `map`:
|
||||
Map files are located at the `~/catkin_ws/src/clover/aruco_pose/map` directory. Map file name is defined in the `map` argument:
|
||||
|
||||
```xml
|
||||
<param name="map" value="$(find aruco_pose)/map/map.txt"/>
|
||||
<arg name="map" default="map.txt"/>
|
||||
```
|
||||
|
||||
Some map examples are provided in [`~/catkin_ws/src/clover/aruco_pose/map`](https://github.com/CopterExpress/clover/tree/master/aruco_pose/map).
|
||||
Some map examples are provided in [the directory](https://github.com/CopterExpress/clover/tree/master/aruco_pose/map).
|
||||
|
||||
Grid maps may be generated using the `genmap.py` script:
|
||||
|
||||
@@ -152,10 +157,10 @@ If the drone's altitude is not stable, try increasing the `MPC_Z_VEL_P` paramete
|
||||
|
||||
In order to navigate using markers on the ceiling, mount the onboard camera so that it points up and [adjust the camera frame accordingly](camera_setup.md).
|
||||
|
||||
You should also set the `known_tilt` parameter to `map_flipped` in both `aruco_detect` and `aruco_map` sections of `~/catkin_ws/src/clover/clover/launch/aruco.launch`:
|
||||
You should also set the `placement` parameter to `ceilin` in `~/catkin_ws/src/clover/clover/launch/aruco.launch`:
|
||||
|
||||
```xml
|
||||
<param name="known_tilt" value="map_flipped"/>
|
||||
<arg name="placement" default="ceiling"/>
|
||||
```
|
||||
|
||||
This will flip the `aruco_map` frame (making its **<font color=blue>z</font>** axis point downward). Thus, in order to fly 2 metres below ceiling, the `z` argument for the `navigate` service should be set to 2:
|
||||
|
||||
@@ -1,5 +1,9 @@
|
||||
# ArUco marker detection
|
||||
|
||||
> **Note** The following applies to [image versions](image.md) **0.22** and up. Older documentation is still available for [for version **0.20**](https://github.com/CopterExpress/clover/blob/v0.20/docs/en/aruco_marker.md).
|
||||
|
||||
<!-- -->
|
||||
|
||||
> **Info** Marker detection requires the camera module to be correctly plugged in and [configured](camera.md).
|
||||
|
||||
`aruco_detect` module detects ArUco markers and publishes their positions in ROS topics and as [TF frames](frames.md).
|
||||
@@ -22,22 +26,20 @@ For enabling detection set the `aruco_detect` argument in `~/catkin_ws/src/clove
|
||||
<arg name="aruco_detect" default="true"/>
|
||||
```
|
||||
|
||||
For the module to work correctly the following parameters should be set:
|
||||
For the module to work correctly the following arguments should also be set:
|
||||
|
||||
```xml
|
||||
<param name="length" value="0.32"/> <!-- length of a single marker, in meters (excluding the white border) -->
|
||||
<param name="estimate_poses" value="true"/> <!-- position estimation for single markers -->
|
||||
<param name="send_tf" value="true"/> <!-- TF frame creation for markers -->
|
||||
<param name="known_tilt" value="map"/> <!-- Marker tilt, explained below -->
|
||||
<arg name="placement" default="floor"/> <!-- markers' placement, explained below -->
|
||||
<arg name="length" default="0.33"/> <!-- length of a single marker, in meters (excluding the white border) -->
|
||||
```
|
||||
|
||||
`known_tilt` should be set to:
|
||||
`placement` argument should be set to:
|
||||
|
||||
* `map` if *all* markers are on the ground;
|
||||
* `map_flipped` if *all* markers are on the ceiling;
|
||||
* `floor` if *all* markers are on the ground;
|
||||
* `ceiling` if *all* markers are on the ceiling;
|
||||
* an empty string otherwise.
|
||||
|
||||
You may specify length for each marker individually by using the `length_override` parameter:
|
||||
You may specify length for each marker individually by using the `length_override` parameter of the node `aruco_detect`:
|
||||
|
||||
```xml
|
||||
<param name="length_override/3" value="0.1"/> <!-- marker with id=3 has a side of 0.1m -->
|
||||
@@ -98,9 +100,9 @@ rospy.init_node('my_node')
|
||||
# ...
|
||||
|
||||
def markers_callback(msg):
|
||||
print 'Detected markers:':
|
||||
print('Detected markers:'):
|
||||
for marker in msg.markers:
|
||||
print 'Marker: %s' % marker
|
||||
print('Marker: %s' % marker)
|
||||
|
||||
# Create a Subscription object. Each time a message is posted in aruco_detect/markers, the markers_callback function is called with this message as its argument.
|
||||
rospy.Subscriber('aruco_detect/markers', MarkerArray, markers_callback)
|
||||
|
||||
@@ -126,7 +126,7 @@ Ctrl+C
|
||||
Start a program `myprogram.py` using Python:
|
||||
|
||||
```bash
|
||||
python myprogram.py
|
||||
python3 myprogram.py
|
||||
```
|
||||
|
||||
Journal of the events related to `clover` package. Scroll the list by pressing Enter or Ctrl+V (scrolls faster):
|
||||
@@ -411,7 +411,7 @@ The easiest way to send the program is to copy the content of the program, creat
|
||||
- Run the program:
|
||||
|
||||
```bash
|
||||
python my_program.py
|
||||
python3 my_program.py
|
||||
```
|
||||
|
||||
> **Warning** After completion of the program , the drone can land incorrectly and continue to fly over the floor. In this case, you need to intercept control.
|
||||
|
||||
@@ -133,12 +133,12 @@ def image_callback(data):
|
||||
cv_image = bridge.imgmsg_to_cv2(data, 'bgr8') # OpenCV image
|
||||
barcodes = pyzbar.decode(cv_image)
|
||||
for barcode in barcodes:
|
||||
b_data = barcode.data.encode("utf-8")
|
||||
b_data = barcode.data.decode("utf-8")
|
||||
b_type = barcode.type
|
||||
(x, y, w, h) = barcode.rect
|
||||
xc = x + w/2
|
||||
yc = y + h/2
|
||||
print ("Found {} with data {} with center at x={}, y={}".format(b_type, b_data, xc, yc))
|
||||
print("Found {} with data {} with center at x={}, y={}".format(b_type, b_data, xc, yc))
|
||||
|
||||
image_sub = rospy.Subscriber('main_camera/image_raw', Image, image_callback, queue_size=1)
|
||||
|
||||
@@ -153,3 +153,13 @@ The script will take up to 100% CPU capacity. To slow down the script artificial
|
||||
```
|
||||
|
||||
The topic for the subscriber in this case should be changed for `main_camera/image_raw_throttled`.
|
||||
|
||||
## Video recording
|
||||
|
||||
To record a video you can use [`video_recorder`](http://wiki.ros.org/image_view#image_view.2Fdiamondback.video_recorder) node from `image_view` package:
|
||||
|
||||
```bash
|
||||
rosrun image_view video_recorder image:=/main_camera/image_raw
|
||||
```
|
||||
|
||||
The video file will be saved to a file `output.avi`. The `image` argument contains the name of the topic to record.
|
||||
|
||||
@@ -39,7 +39,7 @@ cat file.py
|
||||
Run `file.py` as a Python script:
|
||||
|
||||
```bash
|
||||
python file.py
|
||||
python3 file.py
|
||||
```
|
||||
|
||||
Reboot Raspberry Pi:
|
||||
|
||||
@@ -96,3 +96,7 @@ Prepare your article and send it as a pull request to the [Clover repository](ht
|
||||
## Easy way
|
||||
|
||||
If the above instructions are too difficult for you, send your fixes and new articles by e-mail (<a href="mailto:okalachev@gmail.com">okalachev@gmail.com</a>) or in Telegram messenger (user <a href="tg://resolve?domain=okalachev">@okalachev</a>).
|
||||
|
||||
## Publishing packages
|
||||
|
||||
You also can publish a package, that extends Clover functionality, into the official [COEX Debian repository](packages.md).
|
||||
|
||||
@@ -4,6 +4,8 @@ The RPi image for Clover contains all the necessary software for working with Cl
|
||||
|
||||
## Usage
|
||||
|
||||
> **Info** Starting from version v0.22, the image is based on ROS Noetic and using Python 3. If you want to use ROS Melodic and Python 2, use version [v0.21.2](https://github.com/CopterExpress/clover/releases/download/v0.21.2/clover_v0.21.2.img.zip).
|
||||
|
||||
1. Download the latest stable release of the image – **<a class="latest-image" href="https://github.com/CopterExpress/clover/releases">download</a>**.
|
||||
2. Download and install [Etcher](https://www.balena.io/etcher/), the software for flashing images (available for Windows/Linux/macOS).
|
||||
3. Put the MicroSD-card into your computer (use an adapter if necessary).
|
||||
|
||||
@@ -59,7 +59,7 @@ rospy.init_node('flight')
|
||||
|
||||
def range_callback(msg):
|
||||
# Process data from the rangefinder
|
||||
print 'Rangefinder distance:', msg.range
|
||||
print('Rangefinder distance:', msg.range)
|
||||
|
||||
rospy.Subscriber('rangefinder/range', Range, range_callback)
|
||||
|
||||
|
||||
@@ -70,56 +70,6 @@ The `~/catkin_ws/src/clever/` directory is renamed to `~/catkin_ws/src/clover`.
|
||||
|
||||
For example, `~/catkin_ws/src/clever/clever/launch/clever.launch` file is now `~/catkin_ws/src/clover/clover/launch/clover.launch`.
|
||||
|
||||
<!--
|
||||
## Python 3 transition
|
||||
|
||||
Python 2 is depracated since, January 1st, 2020. The Clover platform moves to Python 3.
|
||||
|
||||
For running flight script instead of `python` command:
|
||||
|
||||
```bash
|
||||
python flight.py
|
||||
```
|
||||
|
||||
use `python3` command:
|
||||
|
||||
```bash
|
||||
python3 flight.py
|
||||
```
|
||||
|
||||
Python 3 has certain syntax differences in comparison with the old version. Instead of `print` *operator*:
|
||||
|
||||
```python
|
||||
print 'Clover is the best'
|
||||
```
|
||||
|
||||
use `print` *function*:
|
||||
|
||||
```python
|
||||
print('Clover is the best')
|
||||
```
|
||||
|
||||
The division operator operates floating points by default (instead of integer). Python 2:
|
||||
|
||||
```python
|
||||
>>> 10 / 4
|
||||
2
|
||||
```
|
||||
|
||||
Python 3:
|
||||
|
||||
```python
|
||||
>>> 10 / 4
|
||||
2.5
|
||||
```
|
||||
|
||||
For strings `unicode` type is used by default (instead of `str` type).
|
||||
|
||||
Encoding specification (`# coding: utf8`) is not necessary any more.
|
||||
|
||||
More details on all the language changes see in [appropriate article](https://sebastianraschka.com/Articles/2014_python_2_3_key_diff.html).
|
||||
-->
|
||||
|
||||
## Wi-Fi network configuration
|
||||
|
||||
Wi-Fi networks' SSID is changed to `clover-XXXX` (where X is a random number), password is changed to `cloverwifi`.
|
||||
|
||||
59
docs/en/migrate22.md
Normal file
59
docs/en/migrate22.md
Normal file
@@ -0,0 +1,59 @@
|
||||
# Migration to version 0.22
|
||||
|
||||
## Python 3 transition
|
||||
|
||||
Python 2 is [deprecated](https://www.python.org/doc/sunset-python-2/) since January 1st, 2020. The Clover platform moves to Python 3.
|
||||
|
||||
For running flight script instead of `python` command:
|
||||
|
||||
```bash
|
||||
python flight.py
|
||||
```
|
||||
|
||||
use `python3` command:
|
||||
|
||||
```bash
|
||||
python3 flight.py
|
||||
```
|
||||
|
||||
Python 3 has certain syntax differences in comparison with the old version. Instead of `print` *operator*:
|
||||
|
||||
```python
|
||||
print 'Clover is the best' # this won't work
|
||||
```
|
||||
|
||||
use `print` *function*:
|
||||
|
||||
```python
|
||||
print('Clover is the best')
|
||||
```
|
||||
|
||||
The division operator operates floating points by default (instead of integer). Python 2:
|
||||
|
||||
```python
|
||||
>>> 10 / 4
|
||||
2
|
||||
```
|
||||
|
||||
Python 3:
|
||||
|
||||
```python
|
||||
>>> 10 / 4
|
||||
2.5
|
||||
```
|
||||
|
||||
For strings `unicode` type is used by default (instead of `str` type).
|
||||
|
||||
Encoding specification (`# coding: utf8`) is not necessary any more.
|
||||
|
||||
More details on all the language changes see in [appropriate article](https://sebastianraschka.com/Articles/2014_python_2_3_key_diff.html).
|
||||
|
||||
## Move to ROS Noetic
|
||||
|
||||
<img src="../assets/noetic.png" width=200>
|
||||
|
||||
ROS Melodic version was updated to ROS Noetic. See the full list of changes in the [ROS official documentation](http://wiki.ros.org/noetic/Migration).
|
||||
|
||||
## Changes in launch-files
|
||||
|
||||
Configuration of ArUco-markers navigation is simplified. See details in [markers navigation](aruco_marker.md) and [markers map navigation](aruco_map.md) articles.
|
||||
27
docs/en/packages.md
Normal file
27
docs/en/packages.md
Normal file
@@ -0,0 +1,27 @@
|
||||
# COEX packages repository
|
||||
|
||||
COEX provides an open [Debian-repository](https://wiki.debian.org/DebianRepository) with ROS Noetic related prebuilt binary pacakges for `armhf` architecture.
|
||||
|
||||
> **Info** Repository URL: http://packages.coex.tech.
|
||||
|
||||
The repository is already addedd in [RPi image](image.md) and may be used for simple installation of additional ROS packages.
|
||||
|
||||
## Packages publishing
|
||||
|
||||
You can make a Pull Request in a git repository with packages, adding or updating your package (a file with `.deb` extension), that relates to Clover or ROS. After merging your package will be available for installation with `apt` utility:
|
||||
|
||||
```bash
|
||||
sudo apt install ros-noetic-clover-some-feature
|
||||
```
|
||||
|
||||
Packages, that extend Clover functionality are recommended to be named with `clover_` prefix, e. g. `clover_some_feature`.
|
||||
|
||||
## Using on a normal Raspberry Pi OS
|
||||
|
||||
On a normal Raspberry Pi OS, the repository may be added to the sources list, this way:
|
||||
|
||||
```bash
|
||||
wget -O - 'http://packages.coex.tech/key.asc' | apt-key add -
|
||||
echo 'deb http://packages.coex.tech buster main' >> /etc/apt/sources.list
|
||||
sudo apt update
|
||||
```
|
||||
@@ -42,10 +42,10 @@ Before the first flight it's recommended to check the Clover's configuration wit
|
||||
rosrun clover selfcheck.py
|
||||
```
|
||||
|
||||
In order to run a Python script use the `python` command:
|
||||
In order to run a Python script use the `python3` command:
|
||||
|
||||
```bash
|
||||
python flight.py
|
||||
python3 flight.py
|
||||
```
|
||||
|
||||
Below is a complete flight program that performs a takeoff, flies forward and lands:
|
||||
|
||||
@@ -63,7 +63,7 @@ An example of subscription to topic `/foo`:
|
||||
|
||||
```python
|
||||
def foo_callback(msg):
|
||||
print msg.data
|
||||
print(msg.data)
|
||||
|
||||
# Subscribing. When a message is received in topic /foo, function foo_callback will be invoked.
|
||||
rospy.Subscriber('/foo', String, foo_callback)
|
||||
|
||||
@@ -75,14 +75,14 @@ Displaying drone coordinates `x`, `y` and `z` in the local system of coordinates
|
||||
|
||||
```python
|
||||
telemetry = get_telemetry()
|
||||
print telemetry.x, telemetry.y, telemetry.z
|
||||
print(telemetry.x, telemetry.y, telemetry.z)
|
||||
```
|
||||
|
||||
Displaying drone altitude relative to [the ArUco map](aruco.md):
|
||||
|
||||
```python
|
||||
telemetry = get_telemetry(frame_id='aruco_map')
|
||||
print telemetry.z
|
||||
print(telemetry.z)
|
||||
```
|
||||
|
||||
Checking global position availability:
|
||||
@@ -90,9 +90,9 @@ Checking global position availability:
|
||||
```python
|
||||
import math
|
||||
if not math.isnan(get_telemetry().lat):
|
||||
print 'Global position is available'
|
||||
print('Global position is available')
|
||||
else:
|
||||
print 'No global position'
|
||||
print('No global position')
|
||||
```
|
||||
|
||||
Output of current telemetry (command line):
|
||||
@@ -303,7 +303,7 @@ Landing the drone:
|
||||
res = land()
|
||||
|
||||
if res.success:
|
||||
print 'drone is landing'
|
||||
print('drone is landing')
|
||||
```
|
||||
|
||||
Landing the drone (command line):
|
||||
|
||||
@@ -319,7 +319,7 @@ def flip():
|
||||
rospy.loginfo('finish flip')
|
||||
set_position(x=start.x, y=start.y, z=start.z, yaw=start.yaw) # finish flip
|
||||
|
||||
print navigate(z=2, speed=1, frame_id='body', auto_arm=True) # take off
|
||||
print(navigate(z=2, speed=1, frame_id='body', auto_arm=True)) # take off
|
||||
rospy.sleep(10)
|
||||
|
||||
rospy.loginfo('flip')
|
||||
|
||||
@@ -83,7 +83,7 @@ pi.callback(ECHO, pigpio.FALLING_EDGE, fall)
|
||||
|
||||
while True:
|
||||
# Reading the distance:
|
||||
print read_distance()
|
||||
print(read_distance())
|
||||
|
||||
```
|
||||
|
||||
@@ -104,7 +104,7 @@ def read_distance_filtered():
|
||||
return numpy.median(history)
|
||||
|
||||
while True:
|
||||
print read_distance_filtered()
|
||||
print(read_distance_filtered())
|
||||
```
|
||||
|
||||
An example of charts of initial and filtered data:
|
||||
|
||||
@@ -97,7 +97,9 @@
|
||||
* [Подключение регулятора 4 в 1](4in1.md)
|
||||
* [Светодиодная лента (legacy)](leds_old.md)
|
||||
* [Вклад в Клевер](contributing.md)
|
||||
* [Репозиторий пакетов COEX](packages.md)
|
||||
* [Переход на версию 0.20](migrate20.md)
|
||||
* [Переход на версию 0.22](migrate22.md)
|
||||
* [COEX DuoCam](duocam.md)
|
||||
* [Виртуальная MAVLink-камера](duocam_mavlink.md)
|
||||
* [Мероприятия](events.md)
|
||||
|
||||
@@ -1,5 +1,9 @@
|
||||
# Навигация по картам ArUco-маркеров
|
||||
|
||||
> **Note** Документация для версий [образа](image.md), начиная с версии **0.22**. Для более ранних версий см. [документацию для версии **0.20**](https://github.com/CopterExpress/clover/blob/v0.20/docs/ru/aruco_map.md).
|
||||
|
||||
<!-- -->
|
||||
|
||||
> **Info** Для распознавания маркеров модуль камеры должен быть корректно подключен и [сконфигурирован](camera.md).
|
||||
|
||||
<!-- -->
|
||||
@@ -39,13 +43,13 @@ id_маркера размер_маркера x y z угол_z угол_y уго
|
||||
|
||||
Где `угол_N` – это угол поворота маркера вокруг оси N в радианах.
|
||||
|
||||
Путь к файлу с картой задается в параметре `map`:
|
||||
Файлы карт располагаются в каталоге `~/catkin_ws/src/clover/aruco_pose/map`. Название файла с картой задается в аргументе `map`:
|
||||
|
||||
```xml
|
||||
<param name="map" value="$(find aruco_pose)/map/map.txt"/>
|
||||
<arg name="map" default="map.txt"/>
|
||||
```
|
||||
|
||||
Смотрите примеры карт маркеров в каталоге [`~/catkin_ws/src/clover/aruco_pose/map`](https://github.com/CopterExpress/clover/tree/master/aruco_pose/map).
|
||||
Смотрите примеры карт маркеров в [`вышеуказанном каталоге`](https://github.com/CopterExpress/clover/tree/master/aruco_pose/map).
|
||||
|
||||
Файл карты может быть сгенерирован с помощью инструмента `genmap.py`:
|
||||
|
||||
@@ -154,10 +158,10 @@ navigate(frame_id='aruco_5', x=0, y=0, z=1)
|
||||
|
||||
Для навигации по маркерам, расположенным на потолке, необходимо поставить основную камеру так, чтобы она смотрела вверх и [установить соответствующий фрейм камеры](camera_setup.md#frame).
|
||||
|
||||
Также в файле `~/catkin_ws/src/clover/clover/launch/aruco.launch` необходимо установить параметр `known_tilt` в секциях `aruco_detect` и `aruco_map` в значение `map_flipped`:
|
||||
Также в файле `~/catkin_ws/src/clover/clover/launch/aruco.launch` необходимо выставить аргумент `placement` в значение `ceiling`:
|
||||
|
||||
```xml
|
||||
<param name="known_tilt" value="map_flipped"/>
|
||||
<arg name="placement" default="ceiling"/>
|
||||
```
|
||||
|
||||
При такой конфигурации фрейм `aruco_map` также окажется перевернутым. Таким образом, для полета на высоту 2 метра ниже потолка, аргумент `z` нужно устанавливать в 2:
|
||||
|
||||
@@ -1,5 +1,9 @@
|
||||
# Распознавание ArUco-маркеров
|
||||
|
||||
> **Note** Документация для версий [образа](image.md), начиная с версии **0.22**. Для более ранних версий см. [документацию для версии **0.20**](https://github.com/CopterExpress/clover/blob/v0.20/docs/ru/aruco_marker.md).
|
||||
|
||||
<!-- -->
|
||||
|
||||
> **Info** Для распознавания маркеров модуль камеры должен быть корректно подключен и [сконфигурирован](camera_setup.md).
|
||||
|
||||
Модуль `aruco_detect` распознает ArUco-маркеры и публикует их позиции в ROS-топики и в [TF](frames.md).
|
||||
@@ -22,22 +26,20 @@
|
||||
<arg name="aruco_detect" default="true"/>
|
||||
```
|
||||
|
||||
Для правильной работы в этом же файле в секции `aruco_detect` должны быть выставлены параметры:
|
||||
Для правильной работы в этом же файле также должны быть выставлены аргументы:
|
||||
|
||||
```xml
|
||||
<param name="length" value="0.32"/> <!-- размер маркеров в метрах (не включая белую рамку) -->
|
||||
<param name="estimate_poses" value="true"/> <!-- включение вычисления позиций маркеров -->
|
||||
<param name="send_tf" value="true"/> <!-- отправлять позиции маркеров в виде TF-фреймов -->
|
||||
<param name="known_tilt" value="map"/> <!-- наклон маркеров, см. далее -->
|
||||
<arg name="placement" default="floor"/> <!-- расположение маркеров, см. далее -->
|
||||
<arg name="length" default="0.33"/> <!-- размер маркеров в метрах (не включая белую рамку) -->
|
||||
```
|
||||
|
||||
Значение параметра `known_tilt` следует выставлять следующим образом:
|
||||
Значение аргумента `placement` следует выставлять следующим образом:
|
||||
|
||||
* если *все* маркеры наклеены на полу (земле), выставить значение `map`;
|
||||
* если *все* маркеры наклеены на потолке, выставить значение `map_flipped`;
|
||||
* если *все* маркеры наклеены на полу (земле), выставить значение `floor`;
|
||||
* если *все* маркеры наклеены на потолке, выставить значение `ceiling`;
|
||||
* в противном случае удалить строку с параметром.
|
||||
|
||||
Если некоторые маркеры имеют размер, отличный значения `length`, их размер может быть переопределен с помощью параметра `length_override`:
|
||||
Если некоторые маркеры имеют размер, отличный значения `length`, их размер может быть переопределен с помощью параметра `length_override` ноды `aruco_detect`:
|
||||
|
||||
```xml
|
||||
<param name="length_override/3" value="0.1"/> <!-- маркер c id 3 имеет размер 10 см -->
|
||||
@@ -110,9 +112,9 @@ rospy.init_node('my_node')
|
||||
# ...
|
||||
|
||||
def markers_callback(msg):
|
||||
print 'Detected markers:':
|
||||
print('Detected markers:'):
|
||||
for marker in msg.markers:
|
||||
print 'Marker: %s' % marker
|
||||
print('Marker: %s' % marker)
|
||||
|
||||
# Подписываемся. При получении сообщения в топик aruco_detect/markers будет вызвана функция markers_callback.
|
||||
rospy.Subscriber('aruco_detect/markers', MarkerArray, markers_callback)
|
||||
|
||||
@@ -126,7 +126,7 @@ Ctrl+C
|
||||
Запустить программу myprogram.py на Питоне:
|
||||
|
||||
```bash
|
||||
python myprogram.py
|
||||
python3 myprogram.py
|
||||
```
|
||||
|
||||
Журнал событий процессов Клевера. Пролистывать список можно нажатием Enter или сочетанием клавиш Ctrl+V (пролистывает быстрее):
|
||||
@@ -406,7 +406,7 @@ sudo nano /etc/sudoers
|
||||
- Запустите программу. Для этого выполните команду:
|
||||
|
||||
```bash
|
||||
python my_program.py
|
||||
python3 my_program.py
|
||||
```
|
||||
|
||||
> **Warning** После выполнения программы дрон может некорректно приземлиться и продолжать лететь над полом. В таком случае нужно перехватить управление.
|
||||
|
||||
@@ -135,12 +135,12 @@ def image_callback(data):
|
||||
cv_image = bridge.imgmsg_to_cv2(data, 'bgr8') # OpenCV image
|
||||
barcodes = pyzbar.decode(cv_image)
|
||||
for barcode in barcodes:
|
||||
b_data = barcode.data.encode("utf-8")
|
||||
b_data = barcode.data.decode("utf-8")
|
||||
b_type = barcode.type
|
||||
(x, y, w, h) = barcode.rect
|
||||
xc = x + w/2
|
||||
yc = y + h/2
|
||||
print ("Found {} with data {} with center at x={}, y={}".format(b_type, b_data, xc, yc))
|
||||
print("Found {} with data {} with center at x={}, y={}".format(b_type, b_data, xc, yc))
|
||||
|
||||
image_sub = rospy.Subscriber('main_camera/image_raw', Image, image_callback, queue_size=1)
|
||||
|
||||
@@ -155,3 +155,13 @@ rospy.spin()
|
||||
```
|
||||
|
||||
Топик для подписчика в этом случае необходимо поменять на `main_camera/image_raw_throttled`.
|
||||
|
||||
## Запись видео
|
||||
|
||||
Для записи видео может использована нода [`video_recorder`](http://wiki.ros.org/image_view#image_view.2Fdiamondback.video_recorder) из пакета `image_view`:
|
||||
|
||||
```bash
|
||||
rosrun image_view video_recorder image:=/main_camera/image_raw
|
||||
```
|
||||
|
||||
Видео будет сохранено в файл `output.avi`. В аргументе `image` указывается название топика для записи видео.
|
||||
|
||||
@@ -39,7 +39,7 @@ cat file.py
|
||||
Запустить Python-скрипт `file.py`:
|
||||
|
||||
```bash
|
||||
python file.py
|
||||
python3 file.py
|
||||
```
|
||||
|
||||
Перезагрузить Raspberry Pi:
|
||||
|
||||
@@ -96,3 +96,7 @@
|
||||
## Простой способ
|
||||
|
||||
Если вышеприведенные инструкции для вас оказываются слишком сложными, отправляйте правки или новые статьи по e-mail (<a href="mailto:okalachev@gmail.com">okalachev@gmail.com</a>) или в Telegram (пользователь <a href="tg://resolve?domain=okalachev">@okalachev</a>).
|
||||
|
||||
## Публикация пакетов
|
||||
|
||||
Вы также можете опубликовать собственный пакет, расширяющий функциональность Клевера, в [Debian-репозитории COEX](packages.md).
|
||||
|
||||
@@ -4,6 +4,8 @@
|
||||
|
||||
## Использование
|
||||
|
||||
> **Info** Начиная с версии v0.22, образ основан на ROS Noetic и использует Python 3. Если вы хотите использовать ROS Melodic и Python 2, используйте версию [v0.21.2](https://github.com/CopterExpress/clover/releases/download/v0.21.2/clover_v0.21.2.img.zip).
|
||||
|
||||
1. Скачайте последний стабильный релиз образа — **<a class="latest-image" href="https://github.com/CopterExpress/clover/releases">скачать</a>**.
|
||||
2. Скачайте и установите [программу для записи образов Etcher](https://www.balena.io/etcher/) (доступна для Windows/Linux/macOS).
|
||||
3. Установите MicroSD-карту в компьютер (используйте адаптер при необходимости).
|
||||
|
||||
@@ -59,7 +59,7 @@ rospy.init_node('flight')
|
||||
|
||||
def range_callback(msg):
|
||||
# Обработка новых данных с дальномера
|
||||
print 'Rangefinder distance:', msg.range
|
||||
print('Rangefinder distance:', msg.range)
|
||||
|
||||
rospy.Subscriber('rangefinder/range', Range, range_callback)
|
||||
|
||||
|
||||
@@ -72,56 +72,6 @@ sudo systemctl restart clover
|
||||
|
||||
Например, файл `~/catkin_ws/src/clever/clever/launch/clever.launch` теперь называется `~/catkin_ws/src/clover/clover/launch/clover.launch`.
|
||||
|
||||
<!--
|
||||
## Переход на Python 3
|
||||
|
||||
Python 2 был признан [устаревшим](https://www.python.org/doc/sunset-python-2/), начиная с 1 января 2020 года. Платформа Клевера переходит на использование Python 3.
|
||||
|
||||
Для запуска полетных скриптов вместо команды `python`:
|
||||
|
||||
```bash
|
||||
python flight.py
|
||||
```
|
||||
|
||||
теперь следует использовать команду `python3`:
|
||||
|
||||
```bash
|
||||
python3 flight.py
|
||||
```
|
||||
|
||||
Синтаксис языка Python 3 имеет определенные изменения по сравнения со второй версией. Вместо *оператора* `print`:
|
||||
|
||||
```python
|
||||
print 'Clover is the best'
|
||||
```
|
||||
|
||||
теперь используется *функция* `print`:
|
||||
|
||||
```python
|
||||
print('Clover is the best')
|
||||
```
|
||||
|
||||
Оператор деления по умолчанию выполняет деление с плавающей точкой (вместо целочисленного). Python 2:
|
||||
|
||||
```python
|
||||
>>> 10 / 4
|
||||
2
|
||||
```
|
||||
|
||||
Python 3:
|
||||
|
||||
```python
|
||||
>>> 10 / 4
|
||||
2.5
|
||||
```
|
||||
|
||||
Для строк по умолчанию теперь используется тип `unicode` (вместо типа `str`).
|
||||
|
||||
Указание кодировки файла (`# coding: utf8`) перестало быть необходимым.
|
||||
|
||||
Полное описание всех изменений языка смотрите в [соответствующей статье](https://pythonworld.ru/osnovy/python2-vs-python3-razlichiya-sintaksisa.html).
|
||||
-->
|
||||
|
||||
## Настройки Wi-Fi сети
|
||||
|
||||
SSID Wi-Fi сети изменен на `clover-XXXX` (где X – случайная цифра), пароль изменен на `cloverwifi`.
|
||||
|
||||
59
docs/ru/migrate22.md
Normal file
59
docs/ru/migrate22.md
Normal file
@@ -0,0 +1,59 @@
|
||||
# Переход на версию 0.22
|
||||
|
||||
## Переход на Python 3
|
||||
|
||||
Python 2 был признан [устаревшим](https://www.python.org/doc/sunset-python-2/), начиная с 1 января 2020 года. Платформа Клевера переходит на использование Python 3.
|
||||
|
||||
Для запуска полетных скриптов вместо команды `python`:
|
||||
|
||||
```bash
|
||||
python flight.py
|
||||
```
|
||||
|
||||
теперь следует использовать команду `python3`:
|
||||
|
||||
```bash
|
||||
python3 flight.py
|
||||
```
|
||||
|
||||
Синтаксис языка Python 3 имеет определенные изменения по сравнения со второй версией. Вместо *оператора* `print`:
|
||||
|
||||
```python
|
||||
print 'Clover is the best' # this won't work
|
||||
```
|
||||
|
||||
теперь используется *функция* `print`:
|
||||
|
||||
```python
|
||||
print('Clover is the best')
|
||||
```
|
||||
|
||||
Оператор деления по умолчанию выполняет деление с плавающей точкой (вместо целочисленного). Python 2:
|
||||
|
||||
```python
|
||||
>>> 10 / 4
|
||||
2
|
||||
```
|
||||
|
||||
Python 3:
|
||||
|
||||
```python
|
||||
>>> 10 / 4
|
||||
2.5
|
||||
```
|
||||
|
||||
Для строк по умолчанию теперь используется тип `unicode` (вместо типа `str`).
|
||||
|
||||
Указание кодировки файла (`# coding: utf8`) перестало быть необходимым.
|
||||
|
||||
Полное описание всех изменений языка смотрите в [соответствующей статье](https://pythonworld.ru/osnovy/python2-vs-python3-razlichiya-sintaksisa.html).
|
||||
|
||||
## Переход на ROS Noetic
|
||||
|
||||
<img src="../assets/noetic.png" width=200>
|
||||
|
||||
Версия ROS Melodic обновлена до ROS Noetic. Смотрите полный список изменений в [официальной документации ROS](http://wiki.ros.org/noetic/Migration).
|
||||
|
||||
## Изменения в launch-файлах
|
||||
|
||||
Упрощено конфигурирование навигации с использованием ArUco-маркеров. Подробнее в статьях по [навигации по маркерам](aruco_marker.md) и [навигации по картам маркеров](aruco_map.md).
|
||||
27
docs/ru/packages.md
Normal file
27
docs/ru/packages.md
Normal file
@@ -0,0 +1,27 @@
|
||||
# Репозиторий пакетов COEX
|
||||
|
||||
COEX предоставляет открытый [Debian-репозиторий](https://wiki.debian.org/ru/SourcesList) с предсобранными пакетами, относящимися к ROS Noetic, для архитектуры `armhf`.
|
||||
|
||||
> **Info** Адрес репозитория: http://packages.coex.tech.
|
||||
|
||||
Репозиторий подключен в [образе для RPi](image.md) и может быть использован для легкой установки дополнительных ROS-пакетов.
|
||||
|
||||
## Публикация пакетов
|
||||
|
||||
Вы можете прислать Pull Request в [git-репозиторий с пакетами](https://github.com/CopterExpress/packages), добавляющий или обновляющий ваш пакет (файл с расширением `.deb`), относящийся с Клеверу или ROS. После принятия ваш пакет будет доступен для установки с помощью утилиты `apt`:
|
||||
|
||||
```bash
|
||||
sudo apt install ros-noetic-clover-some-feature
|
||||
```
|
||||
|
||||
Пакеты, расширяющие функциональность Клевера, рекомендуется называть с префиксом `clover_`, например `clover_some_feature`.
|
||||
|
||||
## Использование на обычной Raspberry Pi OS
|
||||
|
||||
На обычной Raspberry Pi OS репозиторий может быть добавлен в список источников пакетов следующими командами:
|
||||
|
||||
```bash
|
||||
wget -O - 'http://packages.coex.tech/key.asc' | apt-key add -
|
||||
echo 'deb http://packages.coex.tech buster main' >> /etc/apt/sources.list
|
||||
sudo apt update
|
||||
```
|
||||
@@ -42,10 +42,10 @@
|
||||
rosrun clover selfcheck.py
|
||||
```
|
||||
|
||||
Для того, чтобы запустить Python-скрипт, используйте команду `python`:
|
||||
Для того, чтобы запустить Python-скрипт, используйте команду `python3`:
|
||||
|
||||
```bash
|
||||
python flight.py
|
||||
python3 flight.py
|
||||
```
|
||||
|
||||
Пример программы для полета (взлет, пролет вперед, посадка):
|
||||
|
||||
@@ -63,7 +63,7 @@ foo_pub.publish(data='Hello, world!') # публикуем сообщение
|
||||
|
||||
```python
|
||||
def foo_callback(msg):
|
||||
print msg.data
|
||||
print(msg.data)
|
||||
|
||||
# Подписываемся. При получении сообщения в топик /foo будет вызвана функция foo_callback.
|
||||
rospy.Subscriber('/foo', String, foo_callback)
|
||||
|
||||
@@ -75,14 +75,14 @@ land = rospy.ServiceProxy('land', Trigger)
|
||||
|
||||
```python
|
||||
telemetry = get_telemetry()
|
||||
print telemetry.x, telemetry.y, telemetry.z
|
||||
print(telemetry.x, telemetry.y, telemetry.z)
|
||||
```
|
||||
|
||||
Вывод высоты коптера относительно [карты ArUco-меток](aruco.md):
|
||||
|
||||
```python
|
||||
telemetry = get_telemetry(frame_id='aruco_map')
|
||||
print telemetry.z
|
||||
print(telemetry.z)
|
||||
```
|
||||
|
||||
Проверка доступности глобальной позиции:
|
||||
@@ -90,9 +90,9 @@ print telemetry.z
|
||||
```python
|
||||
import math
|
||||
if not math.isnan(get_telemetry().lat):
|
||||
print 'Global position is available'
|
||||
print('Global position is available')
|
||||
else:
|
||||
print 'No global position'
|
||||
print('No global position')
|
||||
```
|
||||
|
||||
Вывод текущей телеметрии (командная строка):
|
||||
@@ -303,7 +303,7 @@ set_velocity(vx=1, vy=0.0, vz=0, frame_id='body')
|
||||
res = land()
|
||||
|
||||
if res.success:
|
||||
print 'Copter is landing'
|
||||
print('Copter is landing')
|
||||
```
|
||||
|
||||
Посадка коптера (командная строка):
|
||||
|
||||
@@ -337,7 +337,7 @@ def flip():
|
||||
rospy.loginfo('finish flip')
|
||||
set_position(x=start.x, y=start.y, z=start.z, yaw=start.yaw) # finish flip
|
||||
|
||||
print navigate(z=2, speed=1, frame_id='body', auto_arm=True) # take off
|
||||
print(navigate(z=2, speed=1, frame_id='body', auto_arm=True)) # take off
|
||||
rospy.sleep(10)
|
||||
|
||||
rospy.loginfo('flip')
|
||||
|
||||
@@ -83,7 +83,7 @@ pi.callback(ECHO, pigpio.FALLING_EDGE, fall)
|
||||
|
||||
while True:
|
||||
# Читаем дистанцию:
|
||||
print read_distance()
|
||||
print(read_distance())
|
||||
|
||||
```
|
||||
|
||||
@@ -104,7 +104,7 @@ def read_distance_filtered():
|
||||
return numpy.median(history)
|
||||
|
||||
while True:
|
||||
print read_distance_filtered()
|
||||
print(read_distance_filtered())
|
||||
```
|
||||
|
||||
Пример графиков исходных и отфильтрованных данных:
|
||||
|
||||
@@ -6,3 +6,7 @@ find_package(catkin REQUIRED)
|
||||
catkin_package()
|
||||
|
||||
install(DIRECTORY launch DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION})
|
||||
|
||||
catkin_install_python(PROGRAMS main.py
|
||||
DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
|
||||
)
|
||||
|
||||
Reference in New Issue
Block a user