diff --git a/.github/workflows/build.yml b/.github/workflows/build.yml index 0709e465..2d409811 100644 --- a/.github/workflows/build.yml +++ b/.github/workflows/build.yml @@ -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 diff --git a/.markdownlint.json b/.markdownlint.json index 9c2ab472..6ca8b315 100644 --- a/.markdownlint.json +++ b/.markdownlint.json @@ -21,6 +21,7 @@ "ROS", "ROS Kinetic", "ROS Melodic", + "ROS Noetic", "OpenCV", "OpenVPN", "Gazebo", diff --git a/README.md b/README.md index 8ff3b1b5..65445daf 100644 --- a/README.md +++ b/README.md @@ -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) diff --git a/aruco_pose/CMakeLists.txt b/aruco_pose/CMakeLists.txt index f0176424..e3ab2a93 100644 --- a/aruco_pose/CMakeLists.txt +++ b/aruco_pose/CMakeLists.txt @@ -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 ## ############# diff --git a/aruco_pose/src/aruco_detect.cpp b/aruco_pose/src/aruco_detect.cpp index f1658ac7..ae945876 100644 --- a/aruco_pose/src/aruco_detect.cpp +++ b/aruco_pose/src/aruco_detect.cpp @@ -112,10 +112,7 @@ public: image_transport::ImageTransport it_priv(nh_priv_); dyn_srv_ = std::make_shared>(nh_priv_); - dynamic_reconfigure::Server::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("markers", 1); diff --git a/aruco_pose/src/draw.cpp b/aruco_pose/src/draw.cpp index d9cd0e7d..d8e7b021 100644 --- a/aruco_pose/src/draw.cpp +++ b/aruco_pose/src/draw.cpp @@ -3,26 +3,11 @@ #include "draw.h" #include +#include 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 +static std::vector worldToCamera(const std::vector& 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 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(point.x * rmat.at(0,0) + point.y * rmat.at(0,1) + point.z * rmat.at(0,2) + tvec_64f.at(0)), + static_cast(point.x * rmat.at(1,0) + point.y * rmat.at(1,1) + point.z * rmat.at(1,2) + tvec_64f.at(1)), + static_cast(point.x * rmat.at(2,0) + point.y * rmat.at(2,1) + point.z * rmat.at(2,2) + tvec_64f.at(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 +static std::vector cameraToScreen(const std::vector& 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 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(0) * r2 + dc_64f.at(1) * r4 + dc_64f.at(4) * r6; + // polynomial distortion factror (denominator) + double pddist = 1.0 / (1 + dc_64f.at(5) * r2 + dc_64f.at(6) * r4 + dc_64f.at(7) * r6); + // Distorted point coordinates (always double-precision) + cv::Point3d distortedPoint{ + camPoint.x * pndist * pddist + dc_64f.at(2) * tg1 + dc_64f.at(3) * tg2 + dc_64f.at(8) * r2 + dc_64f.at(9) * r4, + camPoint.y * pndist * pddist + dc_64f.at(2) * tg3 + dc_64f.at(3) * tg1 + dc_64f.at(10) * r2 + dc_64f.at(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(distortedPoint.x * cm_64f.at(0, 0) + cm_64f.at(0, 2)), + static_cast(distortedPoint.y * cm_64f.at(1, 1) + cm_64f.at(1, 2)), + static_cast(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 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((1 - alpha) * p1.x + alpha * p2.x), + static_cast((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 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 - void computeTiltProjectionMatrix(FLOAT tauX, - FLOAT tauY, - Matx* matTilt = 0, - Matx* dMatTiltdTauX = 0, - Matx* dMatTiltdTauY = 0, - Matx* invMatTilt = 0) - { - FLOAT cTauX = cos(tauX); - FLOAT sTauX = sin(tauX); - FLOAT cTauY = cos(tauY); - FLOAT sTauY = sin(tauY); - Matx matRotX = Matx(1,0,0,0,cTauX,sTauX,0,-sTauX,cTauX); - Matx matRotY = Matx(cTauY,0,-sTauY,0,1,0,sTauY,0,cTauY); - Matx matRotXY = matRotY * matRotX; - Matx matProjZ = Matx(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 dMatRotXYdTauX = matRotY * Matx(0,0,0,0,-sTauX,cTauX,0,-cTauX,-sTauX); - Matx dMatProjZdTauX = Matx(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 dMatRotXYdTauY = Matx(-sTauY,0,-cTauY,0,0,0,cTauY,0,-sTauY) * matRotX; - Matx dMatProjZdTauY = Matx(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 invMatProjZ = Matx(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 matM, _m; - Ptr _dpdr, _dpdt, _dpdc, _dpdf, _dpdk; - Ptr _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 ); } diff --git a/aruco_pose/vendor/VendorOpenCV.cmake b/aruco_pose/vendor/VendorOpenCV.cmake index fc0bac5f..4b8f44bb 100644 --- a/aruco_pose/vendor/VendorOpenCV.cmake +++ b/aruco_pose/vendor/VendorOpenCV.cmake @@ -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 diff --git a/aruco_pose/vendor/aruco/src/apriltag_quad_thresh.cpp b/aruco_pose/vendor/aruco/src/apriltag_quad_thresh.cpp index fc75a384..06170619 100644 --- a/aruco_pose/vendor/aruco/src/apriltag_quad_thresh.cpp +++ b/aruco_pose/vendor/aruco/src/apriltag_quad_thresh.cpp @@ -94,7 +94,7 @@ void ptsort_(struct pt *pts, int sz) // Use stack storage if it's not too big. cv::AutoBuffer _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 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 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 &_params, const Mat im, zarray_t *clu // efficiently computed for any contiguous range of indices. cv::AutoBuffer 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; diff --git a/builder/assets/clever/setup.py b/builder/assets/clever/setup.py index 1ed75dee..d2169cdb 100755 --- a/builder/assets/clever/setup.py +++ b/builder/assets/clever/setup.py @@ -1,4 +1,4 @@ -#!/usr/bin/env python +#!/usr/bin/env python3 from distutils.core import setup diff --git a/builder/assets/noetic-rosdep-clover.yaml b/builder/assets/noetic-rosdep-clover.yaml new file mode 100644 index 00000000..a595b87e --- /dev/null +++ b/builder/assets/noetic-rosdep-clover.yaml @@ -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] diff --git a/builder/assets/roscore.service b/builder/assets/roscore.service index 81718289..b749e887 100644 --- a/builder/assets/roscore.service +++ b/builder/assets/roscore.service @@ -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 diff --git a/builder/image-build.sh b/builder/image-build.sh index db7be310..082b01a7 100755 --- a/builder/image-build.sh +++ b/builder/image-build.sh @@ -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/' diff --git a/builder/image-ros.sh b/builder/image-ros.sh index b9700088..b8016083 100755 --- a/builder/image-ros.sh +++ b/builder/image-ros.sh @@ -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 # 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 diff --git a/builder/image-software.sh b/builder/image-software.sh index d0686b2e..c675c913 100755 --- a/builder/image-software.sh +++ b/builder/image-software.sh @@ -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 diff --git a/builder/image-validate.sh b/builder/image-validate.sh index a95beecd..8d13f0f6 100755 --- a/builder/image-validate.sh +++ b/builder/image-validate.sh @@ -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 diff --git a/builder/standalone-install.sh b/builder/standalone-install.sh index e826401a..7a8ae535 100755 --- a/builder/standalone-install.sh +++ b/builder/standalone-install.sh @@ -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 < /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 diff --git a/builder/test/qr.png b/builder/test/qr.png new file mode 100644 index 00000000..4044d85d Binary files /dev/null and b/builder/test/qr.png differ diff --git a/builder/test/test_qr.py b/builder/test/test_qr.py new file mode 100755 index 00000000..99f0fd2f --- /dev/null +++ b/builder/test/test_qr.py @@ -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() diff --git a/builder/test/tests.py b/builder/test/tests.py index 4fd42aa8..2af19852 100755 --- a/builder/test/tests.py +++ b/builder/test/tests.py @@ -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()) diff --git a/builder/test/tests.sh b/builder/test/tests.sh index f4404346..a0c27418 100755 --- a/builder/test/tests.sh +++ b/builder/test/tests.sh @@ -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/*) ]] diff --git a/builder/test/tests_clever.py b/builder/test/tests_clever.py index 8db8b6b0..ffe251cc 100755 --- a/builder/test/tests_clever.py +++ b/builder/test/tests_clever.py @@ -1,4 +1,4 @@ -#!/usr/bin/env python +#!/usr/bin/env python3 # test backwards compatibility diff --git a/clover/CMakeLists.txt b/clover/CMakeLists.txt index d779e5f7..2e96289e 100644 --- a/clover/CMakeLists.txt +++ b/clover/CMakeLists.txt @@ -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) diff --git a/clover/README.md b/clover/README.md index e3fd3147..51243812 100644 --- a/clover/README.md +++ b/clover/README.md @@ -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`: diff --git a/clover/launch/aruco.launch b/clover/launch/aruco.launch index c4ce5781..dfd69899 100644 --- a/clover/launch/aruco.launch +++ b/clover/launch/aruco.launch @@ -2,30 +2,37 @@ + + + - + - - + + + + + - + - - + + + diff --git a/clover/launch/clover.launch b/clover/launch/clover.launch index 870d6e18..26ec4454 100644 --- a/clover/launch/clover.launch +++ b/clover/launch/clover.launch @@ -36,18 +36,13 @@ - + - - - - - diff --git a/clover/launch/main_camera.launch b/clover/launch/main_camera.launch index aa52f04d..312a0f15 100644 --- a/clover/launch/main_camera.launch +++ b/clover/launch/main_camera.launch @@ -3,6 +3,7 @@ + @@ -17,9 +18,14 @@ + + + + + - - + + diff --git a/clover/package.xml b/clover/package.xml index 43842cb9..a4f5878b 100644 --- a/clover/package.xml +++ b/clover/package.xml @@ -1,5 +1,5 @@ - + clover 0.21.1 The Clover package @@ -37,7 +37,8 @@ rosbridge_server web_video_server tf2_web_republisher - python-lxml + python-lxml + python3-lxml python-pymavlink diff --git a/clover/src/selfcheck.py b/clover/src/selfcheck.py index 1a3cddf1..25e413e1 100755 --- a/clover/src/selfcheck.py +++ b/clover/src/selfcheck.py @@ -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(): # = # In case of `get_throttled`, 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 diff --git a/clover/test/basic.py b/clover/test/basic.py index f9a767d8..283180ed 100755 --- a/clover/test/basic.py +++ b/clover/test/basic.py @@ -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() diff --git a/clover_blocks/src/clover_blocks b/clover_blocks/src/clover_blocks index 82a12854..4000df8e 100755 --- a/clover_blocks/src/clover_blocks +++ b/clover_blocks/src/clover_blocks @@ -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: diff --git a/clover_blocks/www/python.js b/clover_blocks/www/python.js index b8ee9510..365dfccf 100644 --- a/clover_blocks/www/python.js +++ b/clover_blocks/www/python.js @@ -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`; diff --git a/clover_simulation/src/clover_simulation/__init__.py b/clover_simulation/src/clover_simulation/__init__.py index 30ed091d..72822c53 100644 --- a/clover_simulation/src/clover_simulation/__init__.py +++ b/clover_simulation/src/clover_simulation/__init__.py @@ -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) diff --git a/docs/assets/noetic.png b/docs/assets/noetic.png new file mode 100644 index 00000000..ae4e377a Binary files /dev/null and b/docs/assets/noetic.png differ diff --git a/docs/en/SUMMARY.md b/docs/en/SUMMARY.md index 91b9feb3..9acb42b3 100644 --- a/docs/en/SUMMARY.md +++ b/docs/en/SUMMARY.md @@ -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) diff --git a/docs/en/aruco_map.md b/docs/en/aruco_map.md index 7867d772..5d6a5ff4 100644 --- a/docs/en/aruco_map.md +++ b/docs/en/aruco_map.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 - + ``` -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 - + ``` This will flip the `aruco_map` frame (making its **z** axis point downward). Thus, in order to fly 2 metres below ceiling, the `z` argument for the `navigate` service should be set to 2: diff --git a/docs/en/aruco_marker.md b/docs/en/aruco_marker.md index c75568a4..f2208f25 100644 --- a/docs/en/aruco_marker.md +++ b/docs/en/aruco_marker.md @@ -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 ``` -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 - - - - + + ``` -`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 @@ -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) diff --git a/docs/en/auto_setup.md b/docs/en/auto_setup.md index 17213516..580ec75e 100644 --- a/docs/en/auto_setup.md +++ b/docs/en/auto_setup.md @@ -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. diff --git a/docs/en/camera.md b/docs/en/camera.md index 0308b6ed..5dfe34fa 100644 --- a/docs/en/camera.md +++ b/docs/en/camera.md @@ -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. diff --git a/docs/en/cli.md b/docs/en/cli.md index 6f634f10..3db1fc1e 100644 --- a/docs/en/cli.md +++ b/docs/en/cli.md @@ -39,7 +39,7 @@ cat file.py Run `file.py` as a Python script: ```bash -python file.py +python3 file.py ``` Reboot Raspberry Pi: diff --git a/docs/en/contributing.md b/docs/en/contributing.md index 5f56437e..d25f3124 100644 --- a/docs/en/contributing.md +++ b/docs/en/contributing.md @@ -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 (okalachev@gmail.com) or in Telegram messenger (user @okalachev). + +## Publishing packages + +You also can publish a package, that extends Clover functionality, into the official [COEX Debian repository](packages.md). diff --git a/docs/en/image.md b/docs/en/image.md index ee81e447..454d60be 100644 --- a/docs/en/image.md +++ b/docs/en/image.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 – **download**. 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). diff --git a/docs/en/laser.md b/docs/en/laser.md index 283fd0c1..59dd1cb6 100644 --- a/docs/en/laser.md +++ b/docs/en/laser.md @@ -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) diff --git a/docs/en/migrate20.md b/docs/en/migrate20.md index 8e92f050..2da6aed9 100644 --- a/docs/en/migrate20.md +++ b/docs/en/migrate20.md @@ -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`. - - ## Wi-Fi network configuration Wi-Fi networks' SSID is changed to `clover-XXXX` (where X is a random number), password is changed to `cloverwifi`. diff --git a/docs/en/migrate22.md b/docs/en/migrate22.md new file mode 100644 index 00000000..623dcb52 --- /dev/null +++ b/docs/en/migrate22.md @@ -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 + + + +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. diff --git a/docs/en/packages.md b/docs/en/packages.md new file mode 100644 index 00000000..0e85bffb --- /dev/null +++ b/docs/en/packages.md @@ -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 +``` diff --git a/docs/en/programming.md b/docs/en/programming.md index 78f5ae34..fd72298b 100644 --- a/docs/en/programming.md +++ b/docs/en/programming.md @@ -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: diff --git a/docs/en/ros.md b/docs/en/ros.md index 6753a2dc..ecc1a3e9 100644 --- a/docs/en/ros.md +++ b/docs/en/ros.md @@ -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) diff --git a/docs/en/simple_offboard.md b/docs/en/simple_offboard.md index 05c742a1..d193f283 100644 --- a/docs/en/simple_offboard.md +++ b/docs/en/simple_offboard.md @@ -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): diff --git a/docs/en/snippets.md b/docs/en/snippets.md index b8fe6539..6c8d2127 100644 --- a/docs/en/snippets.md +++ b/docs/en/snippets.md @@ -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') diff --git a/docs/en/sonar.md b/docs/en/sonar.md index f72bbfb4..95ab5fb9 100644 --- a/docs/en/sonar.md +++ b/docs/en/sonar.md @@ -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: diff --git a/docs/ru/SUMMARY.md b/docs/ru/SUMMARY.md index 5f5e655d..b6f4ee2e 100644 --- a/docs/ru/SUMMARY.md +++ b/docs/ru/SUMMARY.md @@ -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) diff --git a/docs/ru/aruco_map.md b/docs/ru/aruco_map.md index 8ef44ace..e34fa73f 100644 --- a/docs/ru/aruco_map.md +++ b/docs/ru/aruco_map.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 - + ``` -Смотрите примеры карт маркеров в каталоге [`~/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 - + ``` При такой конфигурации фрейм `aruco_map` также окажется перевернутым. Таким образом, для полета на высоту 2 метра ниже потолка, аргумент `z` нужно устанавливать в 2: diff --git a/docs/ru/aruco_marker.md b/docs/ru/aruco_marker.md index fee1ffe5..d7599544 100644 --- a/docs/ru/aruco_marker.md +++ b/docs/ru/aruco_marker.md @@ -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 @@ ``` -Для правильной работы в этом же файле в секции `aruco_detect` должны быть выставлены параметры: +Для правильной работы в этом же файле также должны быть выставлены аргументы: ```xml - - - - + + ``` -Значение параметра `known_tilt` следует выставлять следующим образом: +Значение аргумента `placement` следует выставлять следующим образом: -* если *все* маркеры наклеены на полу (земле), выставить значение `map`; -* если *все* маркеры наклеены на потолке, выставить значение `map_flipped`; +* если *все* маркеры наклеены на полу (земле), выставить значение `floor`; +* если *все* маркеры наклеены на потолке, выставить значение `ceiling`; * в противном случае удалить строку с параметром. -Если некоторые маркеры имеют размер, отличный значения `length`, их размер может быть переопределен с помощью параметра `length_override`: +Если некоторые маркеры имеют размер, отличный значения `length`, их размер может быть переопределен с помощью параметра `length_override` ноды `aruco_detect`: ```xml @@ -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) diff --git a/docs/ru/auto_setup.md b/docs/ru/auto_setup.md index 594d0e40..81a5af58 100644 --- a/docs/ru/auto_setup.md +++ b/docs/ru/auto_setup.md @@ -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** После выполнения программы дрон может некорректно приземлиться и продолжать лететь над полом. В таком случае нужно перехватить управление. diff --git a/docs/ru/camera.md b/docs/ru/camera.md index 08ca25e9..522f89ba 100644 --- a/docs/ru/camera.md +++ b/docs/ru/camera.md @@ -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` указывается название топика для записи видео. diff --git a/docs/ru/cli.md b/docs/ru/cli.md index d8c80196..35ed33df 100644 --- a/docs/ru/cli.md +++ b/docs/ru/cli.md @@ -39,7 +39,7 @@ cat file.py Запустить Python-скрипт `file.py`: ```bash -python file.py +python3 file.py ``` Перезагрузить Raspberry Pi: diff --git a/docs/ru/contributing.md b/docs/ru/contributing.md index a183b74e..8b8657f4 100644 --- a/docs/ru/contributing.md +++ b/docs/ru/contributing.md @@ -96,3 +96,7 @@ ## Простой способ Если вышеприведенные инструкции для вас оказываются слишком сложными, отправляйте правки или новые статьи по e-mail (okalachev@gmail.com) или в Telegram (пользователь @okalachev). + +## Публикация пакетов + +Вы также можете опубликовать собственный пакет, расширяющий функциональность Клевера, в [Debian-репозитории COEX](packages.md). diff --git a/docs/ru/image.md b/docs/ru/image.md index 072781ea..4956e586 100644 --- a/docs/ru/image.md +++ b/docs/ru/image.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. Скачайте последний стабильный релиз образа — **скачать**. 2. Скачайте и установите [программу для записи образов Etcher](https://www.balena.io/etcher/) (доступна для Windows/Linux/macOS). 3. Установите MicroSD-карту в компьютер (используйте адаптер при необходимости). diff --git a/docs/ru/laser.md b/docs/ru/laser.md index 7b9c7623..ba54374e 100644 --- a/docs/ru/laser.md +++ b/docs/ru/laser.md @@ -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) diff --git a/docs/ru/migrate20.md b/docs/ru/migrate20.md index 2adf2624..09917e40 100644 --- a/docs/ru/migrate20.md +++ b/docs/ru/migrate20.md @@ -72,56 +72,6 @@ sudo systemctl restart clover Например, файл `~/catkin_ws/src/clever/clever/launch/clever.launch` теперь называется `~/catkin_ws/src/clover/clover/launch/clover.launch`. - - ## Настройки Wi-Fi сети SSID Wi-Fi сети изменен на `clover-XXXX` (где X – случайная цифра), пароль изменен на `cloverwifi`. diff --git a/docs/ru/migrate22.md b/docs/ru/migrate22.md new file mode 100644 index 00000000..780c2121 --- /dev/null +++ b/docs/ru/migrate22.md @@ -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 + + + +Версия ROS Melodic обновлена до ROS Noetic. Смотрите полный список изменений в [официальной документации ROS](http://wiki.ros.org/noetic/Migration). + +## Изменения в launch-файлах + +Упрощено конфигурирование навигации с использованием ArUco-маркеров. Подробнее в статьях по [навигации по маркерам](aruco_marker.md) и [навигации по картам маркеров](aruco_map.md). diff --git a/docs/ru/packages.md b/docs/ru/packages.md new file mode 100644 index 00000000..935b9568 --- /dev/null +++ b/docs/ru/packages.md @@ -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 +``` diff --git a/docs/ru/programming.md b/docs/ru/programming.md index b4e5c395..ef8fe8e8 100644 --- a/docs/ru/programming.md +++ b/docs/ru/programming.md @@ -42,10 +42,10 @@ rosrun clover selfcheck.py ``` -Для того, чтобы запустить Python-скрипт, используйте команду `python`: +Для того, чтобы запустить Python-скрипт, используйте команду `python3`: ```bash -python flight.py +python3 flight.py ``` Пример программы для полета (взлет, пролет вперед, посадка): diff --git a/docs/ru/ros.md b/docs/ru/ros.md index 2704603e..ff11c2e8 100644 --- a/docs/ru/ros.md +++ b/docs/ru/ros.md @@ -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) diff --git a/docs/ru/simple_offboard.md b/docs/ru/simple_offboard.md index a9225c86..ba8a860d 100644 --- a/docs/ru/simple_offboard.md +++ b/docs/ru/simple_offboard.md @@ -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') ``` Посадка коптера (командная строка): diff --git a/docs/ru/snippets.md b/docs/ru/snippets.md index af71488f..e7219bf7 100644 --- a/docs/ru/snippets.md +++ b/docs/ru/snippets.md @@ -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') diff --git a/docs/ru/sonar.md b/docs/ru/sonar.md index 042530f5..7625fad6 100644 --- a/docs/ru/sonar.md +++ b/docs/ru/sonar.md @@ -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()) ``` Пример графиков исходных и отфильтрованных данных: diff --git a/roswww_static/CMakeLists.txt b/roswww_static/CMakeLists.txt index c9334100..bb9e1fc7 100644 --- a/roswww_static/CMakeLists.txt +++ b/roswww_static/CMakeLists.txt @@ -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} +)