diff --git a/.travis.yml b/.travis.yml index 7d496396..ff8e99bd 100644 --- a/.travis.yml +++ b/.travis.yml @@ -73,8 +73,11 @@ jobs: node_js: - "10" before_script: + - sudo sh -c "echo ttf-mscorefonts-installer msttcorefonts/accepted-mscorefonts-eula select true | debconf-set-selections" + - sudo apt update && sudo apt install -y calibre msttcorefonts - npm install gitbook-cli -g - npm install markdownlint-cli -g + - npm install svgexport -g - gitbook -V - markdownlint -V script: @@ -83,18 +86,19 @@ jobs: - ./check_unused_assets.py - gitbook install - gitbook build - # deploy: - # provider: pages - # local_dir: _book - # skip_cleanup: true - # token: ${GITHUB_OAUTH_TOKEN} - # keep_history: true - # target_branch: master - # repo: CopterExpress/clover.coex.tech - # fqdn: clover.coex.tech - # verbose: true - # on: - # branch: master + - gitbook pdf ./ _book/clover.pdf + deploy: + provider: pages + local_dir: _book + skip_cleanup: true + token: ${GITHUB_OAUTH_TOKEN} + keep_history: true + target_branch: master + repo: CopterExpress/clover.coex.tech + fqdn: clover.coex.tech + verbose: true + on: + branch: master - stage: Annotate name: Auto-generate changelog language: python diff --git a/apps/CATKIN_IGNORE b/apps/CATKIN_IGNORE new file mode 100644 index 00000000..e69de29b diff --git a/aruco_pose/CMakeLists.txt b/aruco_pose/CMakeLists.txt index cdd55fd4..30fe1e4b 100644 --- a/aruco_pose/CMakeLists.txt +++ b/aruco_pose/CMakeLists.txt @@ -1,8 +1,6 @@ cmake_minimum_required(VERSION 3.0) project(aruco_pose) -add_definitions(-std=c++11 -Wall -g) - ## Compile as C++11, supported in ROS Kinetic and newer add_compile_options(-std=c++11) @@ -25,7 +23,7 @@ find_package(catkin REQUIRED COMPONENTS ) find_package(OpenCV 3 REQUIRED COMPONENTS core imgproc calib3d) -if ("${OpenCV_VERSION_MINOR}" LESS "3") +if ("${OpenCV_VERSION_MINOR}" LESS "9") message(STATUS "OpenCV version too low, using vendored ArUco package") include(vendor/VendorOpenCV.cmake) else() @@ -229,4 +227,5 @@ if (CATKIN_ENABLE_TESTING) add_rostest(test/test_parser_empty_map.test) add_rostest(test/test_node_failure.test) add_rostest(test/largemap.test) + add_rostest(test/crash_opencv.test) endif() diff --git a/aruco_pose/src/aruco_detect.cpp b/aruco_pose/src/aruco_detect.cpp index 65953e53..7d8d1d99 100644 --- a/aruco_pose/src/aruco_detect.cpp +++ b/aruco_pose/src/aruco_detect.cpp @@ -58,10 +58,9 @@ using cv::Mat; class ArucoDetect : public nodelet::Nodelet { private: - ros::NodeHandle nh_, nh_priv_; - tf2_ros::TransformBroadcaster br_; - tf2_ros::Buffer tf_buffer_; - tf2_ros::TransformListener tf_listener_{tf_buffer_}; + std::unique_ptr br_; + std::unique_ptr tf_buffer_; + std::unique_ptr tf_listener_; std::shared_ptr> dyn_srv_; cv::Ptr dictionary_; cv::Ptr parameters_; @@ -81,30 +80,32 @@ private: public: virtual void onInit() { - nh_ = getNodeHandle(); - nh_priv_ = getPrivateNodeHandle(); + ros::NodeHandle& nh_ = getNodeHandle(); + ros::NodeHandle& nh_priv_ = getPrivateNodeHandle(); + + br_.reset(new tf2_ros::TransformBroadcaster()); + tf_buffer_.reset(new tf2_ros::Buffer()); + tf_listener_.reset(new tf2_ros::TransformListener(*tf_buffer_, nh_)); int dictionary; - nh_priv_.param("dictionary", dictionary, 2); - nh_priv_.param("estimate_poses", estimate_poses_, true); - nh_priv_.param("send_tf", send_tf_, true); + dictionary = nh_priv_.param("dictionary", 2); + estimate_poses_ = nh_priv_.param("estimate_poses", true); + send_tf_ = nh_priv_.param("send_tf", true); if (estimate_poses_ && !nh_priv_.getParam("length", length_)) { NODELET_FATAL("can't estimate marker's poses as ~length parameter is not defined"); ros::shutdown(); } - readLengthOverride(); + readLengthOverride(nh_priv_); - nh_priv_.param("known_tilt", known_tilt_, ""); - nh_priv_.param("auto_flip", auto_flip_, false); + known_tilt_ = nh_priv_.param("known_tilt", ""); + auto_flip_ = nh_priv_.param("auto_flip", false); - nh_priv_.param("frame_id_prefix", frame_id_prefix_, "aruco_"); + frame_id_prefix_ = nh_priv_.param("frame_id_prefix", "aruco_"); camera_matrix_ = cv::Mat::zeros(3, 3, CV_64F); - dist_coeffs_ = cv::Mat::zeros(8, 1, CV_64F); dictionary_ = cv::aruco::getPredefinedDictionary(static_cast(dictionary)); parameters_ = cv::aruco::DetectorParameters::create(); - parameters_->cornerRefinementMethod = cv::aruco::CORNER_REFINE_SUBPIX; image_transport::ImageTransport it(nh_); image_transport::ImageTransport it_priv(nh_priv_); @@ -170,8 +171,8 @@ private: if (!known_tilt_.empty()) { try { - snap_to = tf_buffer_.lookupTransform(msg->header.frame_id, known_tilt_, - msg->header.stamp, ros::Duration(0.02)); + snap_to = tf_buffer_->lookupTransform(msg->header.frame_id, known_tilt_, + msg->header.stamp, ros::Duration(0.02)); } catch (const tf2::TransformException& e) { NODELET_WARN_THROTTLE(5, "can't snap: %s", e.what()); } @@ -205,7 +206,7 @@ private: if (map_markers_ids_.find(ids[i]) == map_markers_ids_.end()) { transform.transform.rotation = marker.pose.orientation; fillTranslation(transform.transform.translation, tvecs[i]); - br_.sendTransform(transform); + br_->sendTransform(transform); } } } @@ -326,10 +327,10 @@ private: return frame_id_prefix_ + std::to_string(id); } - void readLengthOverride() + void readLengthOverride(ros::NodeHandle& nh) { std::map length_override; - nh_priv_.getParam("length_override", length_override); + nh.getParam("length_override", length_override); for (auto const& item : length_override) { length_override_[std::stoi(item.first)] = item.second; } diff --git a/aruco_pose/src/aruco_map.cpp b/aruco_pose/src/aruco_map.cpp index 4896af1e..1193b9bb 100644 --- a/aruco_pose/src/aruco_map.cpp +++ b/aruco_pose/src/aruco_map.cpp @@ -58,7 +58,6 @@ typedef message_filters::sync_policies::ExactTime image_sub_; @@ -83,8 +82,8 @@ private: public: virtual void onInit() { - nh_ = getNodeHandle(); - nh_priv_ = getPrivateNodeHandle(); + ros::NodeHandle &nh_ = getNodeHandle(); + ros::NodeHandle &nh_priv_ = getPrivateNodeHandle(); image_transport::ImageTransport it_priv(nh_priv_); @@ -96,19 +95,18 @@ public: board_->dictionary = cv::aruco::getPredefinedDictionary( static_cast(nh_priv_.param("dictionary", 2))); camera_matrix_ = cv::Mat::zeros(3, 3, CV_64F); - dist_coeffs_ = cv::Mat::zeros(8, 1, CV_64F); std::string type, map; - nh_priv_.param("type", type, "map"); - nh_priv_.param("frame_id", transform_.child_frame_id, "aruco_map"); - nh_priv_.param("known_tilt", known_tilt_, ""); - nh_priv_.param("auto_flip", auto_flip_, false); - nh_priv_.param("image_width", image_width_, 2000); - nh_priv_.param("image_height", image_height_, 2000); - nh_priv_.param("image_margin", image_margin_, 200); - nh_priv_.param("image_axis", image_axis_, true); - nh_priv_.param("markers/frame_id", markers_parent_frame_, transform_.child_frame_id); - nh_priv_.param("markers/child_frame_id_prefix", markers_frame_, ""); + type = nh_priv_.param("type", "map"); + transform_.child_frame_id = nh_priv_.param("frame_id", "aruco_map"); + known_tilt_ = nh_priv_.param("known_tilt", ""); + auto_flip_ = nh_priv_.param("auto_flip", false); + image_width_ = nh_priv_.param("image_width" , 2000); + image_height_ = nh_priv_.param("image_height", 2000); + image_margin_ = nh_priv_.param("image_margin", 200); + image_axis_ = nh_priv_.param("image_axis", true); + markers_parent_frame_ = nh_priv_.param("markers/frame_id", transform_.child_frame_id); + markers_frame_ = nh_priv_.param("markers/child_frame_id_prefix", ""); // createStripLine(); @@ -116,7 +114,7 @@ public: param(nh_priv_, "map", map); loadMap(map); } else if (type == "gridboard") { - createGridBoard(); + createGridBoard(nh_priv_); } else { NODELET_FATAL("unknown type: %s", type.c_str()); ros::shutdown(); @@ -331,7 +329,7 @@ publish_debug: NODELET_INFO("loading %s complete (%d markers)", filename.c_str(), static_cast(board_->ids.size())); } - void createGridBoard() + void createGridBoard(ros::NodeHandle& nh) { NODELET_INFO("generate gridboard"); NODELET_WARN("gridboard maps are deprecated"); @@ -339,15 +337,15 @@ publish_debug: int markers_x, markers_y, first_marker; double markers_side, markers_sep_x, markers_sep_y; std::vector marker_ids; - nh_priv_.param("markers_x", markers_x, 10); - nh_priv_.param("markers_y", markers_y, 10); - nh_priv_.param("first_marker", first_marker, 0); + markers_x = nh.param("markers_x", 10); + markers_y = nh.param("markers_y", 10); + first_marker = nh.param("first_marker", 0); - param(nh_priv_, "markers_side", markers_side); - param(nh_priv_, "markers_sep_x", markers_sep_x); - param(nh_priv_, "markers_sep_y", markers_sep_y); + param(nh, "markers_side", markers_side); + param(nh, "markers_sep_x", markers_sep_x); + param(nh, "markers_sep_y", markers_sep_y); - if (nh_priv_.getParam("marker_ids", marker_ids)) { + if (nh.getParam("marker_ids", marker_ids)) { if ((unsigned int)(markers_x * markers_y) != marker_ids.size()) { NODELET_FATAL("~marker_ids length should be equal to ~markers_x * ~markers_y"); ros::shutdown(); diff --git a/aruco_pose/src/genmap.py b/aruco_pose/src/genmap.py index 0050fc4f..a82b52b8 100755 --- a/aruco_pose/src/genmap.py +++ b/aruco_pose/src/genmap.py @@ -13,7 +13,7 @@ Generate map file for aruco_map nodelet. Usage: - genmap.py [] [--top-left | --bottom-left] + genmap.py [] [] [] [--top-left | --bottom-left] genmap.py (-h | --help) Options: @@ -23,6 +23,8 @@ Options: Distance between markers along X axis Distance between markers along Y axis First marker ID [default: 0] + X coordinate for the first marker [default: 0] + Y coordinate for the first marker [default: 0] --top-left First marker is on top-left (default) --bottom-left First marker is on bottom-left @@ -39,20 +41,22 @@ arguments = docopt(__doc__) length = float(arguments['']) first = int(arguments[''] if arguments[''] is not None else 0) +x0 = float(arguments[''] if arguments[''] is not None else 0) +y0 = float(arguments[''] if arguments[''] is not None else 0) markers_x = int(arguments['']) markers_y = int(arguments['']) dist_x = float(arguments['']) dist_y = float(arguments['']) bottom_left = arguments['--bottom-left'] -max_y = (markers_y - 1) * dist_y +max_y = y0 + (markers_y - 1) * dist_y print('# id\tlength\tx\ty\tz\trot_z\trot_y\trot_x') for y in range(markers_y): for x in range(markers_x): - pos_x = x * dist_x - pos_y = y * dist_y + pos_x = x0 + x * dist_x + pos_y = y0 + y * dist_y if not bottom_left: pos_y = max_y - pos_y - print('{}\t{}\t{}\t{}\t{}\t{}\t{}\t{}\t'.format(first, length, pos_x, pos_y, 0, 0, 0, 0)) + print('{}\t{}\t{}\t{}\t{}\t{}\t{}\t{}'.format(first, length, pos_x, pos_y, 0, 0, 0, 0)) first += 1 diff --git a/aruco_pose/src/utils.h b/aruco_pose/src/utils.h index 3f6ccd16..b7ae0432 100644 --- a/aruco_pose/src/utils.h +++ b/aruco_pose/src/utils.h @@ -35,9 +35,7 @@ static void parseCameraInfo(const sensor_msgs::CameraInfoConstPtr& cinfo, cv::Ma for (unsigned int i = 0; i < 3; ++i) for (unsigned int j = 0; j < 3; ++j) matrix.at(i, j) = cinfo->K[3 * i + j]; - - for (unsigned int k = 0; k < cinfo->D.size(); k++) - dist.at(k) = cinfo->D[k]; + dist = cv::Mat(cinfo->D, true); } inline void rotatePoint(cv::Point3f& p, cv::Point3f origin, float angle) diff --git a/aruco_pose/test/crash_image_01.png b/aruco_pose/test/crash_image_01.png new file mode 100644 index 00000000..dc509792 Binary files /dev/null and b/aruco_pose/test/crash_image_01.png differ diff --git a/aruco_pose/test/crash_image_02.png b/aruco_pose/test/crash_image_02.png new file mode 100644 index 00000000..ac375329 Binary files /dev/null and b/aruco_pose/test/crash_image_02.png differ diff --git a/aruco_pose/test/crash_image_03.png b/aruco_pose/test/crash_image_03.png new file mode 100644 index 00000000..c570402c Binary files /dev/null and b/aruco_pose/test/crash_image_03.png differ diff --git a/aruco_pose/test/crash_opencv.py b/aruco_pose/test/crash_opencv.py new file mode 100644 index 00000000..69c713ab --- /dev/null +++ b/aruco_pose/test/crash_opencv.py @@ -0,0 +1,18 @@ +import rospy +import pytest + +from visualization_msgs.msg import MarkerArray as VisMarkerArray + + +@pytest.fixture +def node(): + return rospy.init_node('aruco_pose_opencv_crash', anonymous=True) + +def test_opencv_crashes_img01(node): + rospy.wait_for_message('aruco_detect_01/visualization', VisMarkerArray, timeout=5) + +def test_opencv_crashes_img02(node): + rospy.wait_for_message('aruco_detect_02/visualization', VisMarkerArray, timeout=5) + +def test_opencv_crashes_img03(node): + rospy.wait_for_message('aruco_detect_03/visualization', VisMarkerArray, timeout=5) diff --git a/aruco_pose/test/crash_opencv.test b/aruco_pose/test/crash_opencv.test new file mode 100644 index 00000000..9d00aa91 --- /dev/null +++ b/aruco_pose/test/crash_opencv.test @@ -0,0 +1,51 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/aruco_pose/vendor/aruco/src/aruco.cpp b/aruco_pose/vendor/aruco/src/aruco.cpp index dfbff73e..2ce00eba 100644 --- a/aruco_pose/vendor/aruco/src/aruco.cpp +++ b/aruco_pose/vendor/aruco/src/aruco.cpp @@ -924,6 +924,8 @@ static void _refineCandidateLines(std::vector& nContours, std::vector + + + + + + + + + %h + + + _sftp-ssh._tcp + 22 + + + diff --git a/builder/assets/monkey b/builder/assets/monkey index d52b2e75..f67ca9cf 100644 --- a/builder/assets/monkey +++ b/builder/assets/monkey @@ -20,7 +20,7 @@ # Example: # DocumentRoot /home/krypton/htdocs - DocumentRoot /home/pi/catkin_ws/src/clover/clover/www + DocumentRoot /home/pi/.ros/www # Redirect: # --------- diff --git a/builder/image-build.sh b/builder/image-build.sh index fc6f6a03..147daa60 100755 --- a/builder/image-build.sh +++ b/builder/image-build.sh @@ -108,6 +108,8 @@ ${BUILDER_DIR}/image-chroot.sh ${IMAGE_PATH} exec ${SCRIPTS_DIR}'/image-software ${BUILDER_DIR}/image-chroot.sh ${IMAGE_PATH} copy ${SCRIPTS_DIR}'/assets/examples' '/home/pi/' # network setup ${BUILDER_DIR}/image-chroot.sh ${IMAGE_PATH} exec ${SCRIPTS_DIR}'/image-network.sh' +# avahi setup +${BUILDER_DIR}/image-chroot.sh ${IMAGE_PATH} copy ${SCRIPTS_DIR}'/assets/avahi-services/sftp-ssh.service' '/etc/avahi/services' # If RPi then use a one thread to build a ROS package on RPi, else use all [[ $(arch) == 'armv7l' ]] && NUMBER_THREADS=1 || NUMBER_THREADS=$(nproc --all) diff --git a/builder/image-ros.sh b/builder/image-ros.sh index 9de629fd..a0168566 100755 --- a/builder/image-ros.sh +++ b/builder/image-ros.sh @@ -143,7 +143,6 @@ echo_stamp "Setup ROS environment" cat << EOF >> /home/pi/.bashrc LANG='C.UTF-8' LC_ALL='C.UTF-8' -ROS_DISTRO='melodic' export ROS_HOSTNAME=\`hostname\`.local source /opt/ros/melodic/setup.bash source /home/pi/catkin_ws/devel/setup.bash diff --git a/builder/image-software.sh b/builder/image-software.sh index 330b8539..a012e037 100755 --- a/builder/image-software.sh +++ b/builder/image-software.sh @@ -57,6 +57,10 @@ my_travis_retry() { return $result } +echo_stamp "Increase apt retries" + +echo "APT::Acquire::Retries \"3\";" > /etc/apt/apt.conf.d/80-retries + echo_stamp "Install apt keys & repos" # TODO: This STDOUT consist 'OK' diff --git a/builder/standalone-install.sh b/builder/standalone-install.sh index 9d9625c5..a10f65a5 100755 --- a/builder/standalone-install.sh +++ b/builder/standalone-install.sh @@ -1,7 +1,7 @@ #!/bin/bash # Perform a "standalone install" in a Docker container - +set -e # Step 1: Install pip apt update apt install -y curl diff --git a/clover/launch/clover.launch b/clover/launch/clover.launch index 97f623dd..41b704ca 100644 --- a/clover/launch/clover.launch +++ b/clover/launch/clover.launch @@ -82,4 +82,9 @@ + + + + + diff --git a/clover/src/optical_flow.cpp b/clover/src/optical_flow.cpp index 8f1a3c0f..616e2464 100644 --- a/clover/src/optical_flow.cpp +++ b/clover/src/optical_flow.cpp @@ -34,9 +34,7 @@ class OpticalFlow : public nodelet::Nodelet { public: OpticalFlow(): - camera_matrix_(3, 3, CV_64F), - dist_coeffs_(8, 1, CV_64F), - tf_listener_(tf_buffer_) + camera_matrix_(3, 3, CV_64F) {} private: @@ -52,8 +50,8 @@ private: Mat hann_; Mat prev_, curr_; Mat camera_matrix_, dist_coeffs_; - tf2_ros::Buffer tf_buffer_; - tf2_ros::TransformListener tf_listener_; + std::unique_ptr tf_buffer_; + std::unique_ptr tf_listener_; bool calc_flow_gyro_; void onInit() @@ -63,11 +61,14 @@ private: image_transport::ImageTransport it(nh); image_transport::ImageTransport it_priv(nh_priv); - nh.param("mavros/local_position/tf/frame_id", local_frame_id_, "map"); - nh.param("mavros/local_position/tf/child_frame_id", fcu_frame_id_, "base_link"); - nh_priv.param("roi", roi_px_, 128); - nh_priv.param("roi_rad", roi_rad_, 0.0); - nh_priv.param("calc_flow_gyro", calc_flow_gyro_, false); + tf_buffer_.reset(new tf2_ros::Buffer()); + tf_listener_.reset(new tf2_ros::TransformListener(*tf_buffer_, nh)); + + local_frame_id_ = nh.param("mavros/local_position/tf/frame_id", "map"); + fcu_frame_id_ = nh.param("mavros/local_position/tf/child_frame_id", "base_link"); + roi_px_ = nh_priv.param("roi", 128); + roi_rad_ = nh_priv.param("roi_rad", 0.0); + calc_flow_gyro_ = nh_priv.param("calc_flow_gyro", false); img_sub_ = it.subscribeCamera("image_raw", 1, &OpticalFlow::flow, this); img_pub_ = it_priv.advertise("debug", 1); @@ -91,9 +92,7 @@ private: camera_matrix_.at(i, j) = cinfo->K[3 * i + j]; } } - for (int k = 0; k < cinfo->D.size(); k++) { - dist_coeffs_.at(k) = cinfo->D[k]; - } + dist_coeffs_ = cv::Mat(cinfo->D, true); } void drawFlow(Mat& frame, double x, double y, double quality) const @@ -186,7 +185,7 @@ private: flow_camera.vector.x = flow_y; // +y means counter-clockwise rotation around Y axis flow_camera.vector.y = -flow_x; // +x means clockwise rotation around X axis try { - tf_buffer_.transform(flow_camera, flow_fcu, fcu_frame_id_); + tf_buffer_->transform(flow_camera, flow_fcu, fcu_frame_id_); } catch (const tf2::TransformException& e) { // transform is not available yet return; @@ -200,7 +199,7 @@ private: try { auto flow_gyro_camera = calcFlowGyro(msg->header.frame_id, prev_stamp_, msg->header.stamp); static geometry_msgs::Vector3Stamped flow_gyro_fcu; - tf_buffer_.transform(flow_gyro_camera, flow_gyro_fcu, fcu_frame_id_); + tf_buffer_->transform(flow_gyro_camera, flow_gyro_fcu, fcu_frame_id_); flow_.integrated_xgyro = flow_gyro_fcu.vector.x; flow_.integrated_ygyro = flow_gyro_fcu.vector.y; flow_.integrated_zgyro = flow_gyro_fcu.vector.z; @@ -247,8 +246,8 @@ private: geometry_msgs::Vector3Stamped calcFlowGyro(const std::string& frame_id, const ros::Time& prev, const ros::Time& curr) { tf2::Quaternion prev_rot, curr_rot; - tf2::fromMsg(tf_buffer_.lookupTransform(frame_id, local_frame_id_, prev).transform.rotation, prev_rot); - tf2::fromMsg(tf_buffer_.lookupTransform(frame_id, local_frame_id_, curr, ros::Duration(0.1)).transform.rotation, curr_rot); + tf2::fromMsg(tf_buffer_->lookupTransform(frame_id, local_frame_id_, prev).transform.rotation, prev_rot); + tf2::fromMsg(tf_buffer_->lookupTransform(frame_id, local_frame_id_, curr, ros::Duration(0.1)).transform.rotation, curr_rot); geometry_msgs::Vector3Stamped flow; flow.header.frame_id = frame_id; diff --git a/clover/src/selfcheck.py b/clover/src/selfcheck.py index 59defb5d..1a3cddf1 100755 --- a/clover/src/selfcheck.py +++ b/clover/src/selfcheck.py @@ -701,7 +701,7 @@ def check_preflight_status(): @check('Network') def check_network(): - ros_hostname = os.environ.get('ROS_HOSTNAME').strip() + ros_hostname = os.environ.get('ROS_HOSTNAME', '').strip() if not ros_hostname: failure('no ROS_HOSTNAME is set') diff --git a/clover/www/CATKIN_IGNORE b/clover/www/CATKIN_IGNORE new file mode 100644 index 00000000..e69de29b diff --git a/clover/www/index.html b/clover/www/index.html index fe3f0def..2234aefc 100644 --- a/clover/www/index.html +++ b/clover/www/index.html @@ -12,8 +12,8 @@