aruco_pose: moar tests

aruco_pose/msg/Marker.msg

@@ -1,4 +1,5 @@
 uint32 id
+float32 length
 geometry_msgs/Pose pose
 Point2D c1
 Point2D c2

aruco_pose/src/aruco_detect.cpp

@@ -169,6 +169,7 @@ private:
 
 			for (unsigned int i = 0; i < ids.size(); i++) {
 				marker.id = ids[i];
+				marker.length = getMarkerLength(marker.id);
 				fillCorners(marker, corners[i]);
 
 				if (estimate_poses_) {

aruco_pose/src/aruco_map.cpp

@@ -399,12 +399,19 @@ publish_debug:
 
 	void publishMapImage()
 	{
+		cv::Size size(image_width_, image_height_);
 		cv::Mat image;
 		cv_bridge::CvImage msg;
-		drawPlanarBoard(board_, cv::Size(image_width_, image_height_), image, image_margin_, 1);
 
-		cv::cvtColor(image, image, CV_GRAY2BGR);
-		msg.encoding = sensor_msgs::image_encodings::BGR8;
+		if (!board_->ids.empty()) {
+			_drawPlanarBoard(board_, size, image, image_margin_, 1);
+		} else {
+			// empty map
+			image.create(size, CV_8UC1);
+			image.setTo(cv::Scalar::all(255));
+		}
+
+		msg.encoding = sensor_msgs::image_encodings::MONO8;
 		msg.image = image;
 		img_pub_.publish(msg.toImageMsg());
 	}

aruco_pose/src/draw.cpp

@@ -6,69 +6,69 @@
 using namespace cv;
 using namespace cv::aruco;
 
-void drawPlanarBoard(Board *_board, Size outSize, OutputArray _img, int marginSize,
-                     int borderBits) {
+void _drawPlanarBoard(Board *_board, Size outSize, OutputArray _img, int marginSize,
+                      int borderBits) {
 
-    CV_Assert(outSize.area() > 0);
-    CV_Assert(marginSize >= 0);
+	CV_Assert(outSize.area() > 0);
+	CV_Assert(marginSize >= 0);
 
-    _img.create(outSize, CV_8UC1);
-    Mat out = _img.getMat();
-    out.setTo(Scalar::all(255));
-    out.adjustROI(-marginSize, -marginSize, -marginSize, -marginSize);
+	_img.create(outSize, CV_8UC1);
+	Mat out = _img.getMat();
+	out.setTo(Scalar::all(255));
+	out.adjustROI(-marginSize, -marginSize, -marginSize, -marginSize);
 
-    // calculate max and min values in XY plane
-    CV_Assert(_board->objPoints.size() > 0);
-    float minX, maxX, minY, maxY;
-    minX = maxX = _board->objPoints[0][0].x;
-    minY = maxY = _board->objPoints[0][0].y;
+	// calculate max and min values in XY plane
+	CV_Assert(_board->objPoints.size() > 0);
+	float minX, maxX, minY, maxY;
+	minX = maxX = _board->objPoints[0][0].x;
+	minY = maxY = _board->objPoints[0][0].y;
 
-    for(unsigned int i = 0; i < _board->objPoints.size(); i++) {
-        for(int j = 0; j < 4; j++) {
-            minX = min(minX, _board->objPoints[i][j].x);
-            maxX = max(maxX, _board->objPoints[i][j].x);
-            minY = min(minY, _board->objPoints[i][j].y);
-            maxY = max(maxY, _board->objPoints[i][j].y);
-        }
-    }
+	for(unsigned int i = 0; i < _board->objPoints.size(); i++) {
+		for(int j = 0; j < 4; j++) {
+			minX = min(minX, _board->objPoints[i][j].x);
+			maxX = max(maxX, _board->objPoints[i][j].x);
+			minY = min(minY, _board->objPoints[i][j].y);
+			maxY = max(maxY, _board->objPoints[i][j].y);
+		}
+	}
 
-    float sizeX = maxX - minX;
-    float sizeY = maxY - minY;
+	float sizeX = maxX - minX;
+	float sizeY = maxY - minY;
 
-    // proportion transformations
-    float xReduction = sizeX / float(out.cols);
-    float yReduction = sizeY / float(out.rows);
+	// proportion transformations
+	float xReduction = sizeX / float(out.cols);
+	float yReduction = sizeY / float(out.rows);
 
-    // determine the zone where the markers are placed
-    if(xReduction > yReduction) {
-        int nRows = int(sizeY / xReduction);
-        int rowsMargins = (out.rows - nRows) / 2;
-        out.adjustROI(-rowsMargins, -rowsMargins, 0, 0);
-    } else {
-        int nCols = int(sizeX / yReduction);
-        int colsMargins = (out.cols - nCols) / 2;
-        out.adjustROI(0, 0, -colsMargins, -colsMargins);
-    }
+	// determine the zone where the markers are placed
+	if(xReduction > yReduction) {
+		int nRows = int(sizeY / xReduction);
+		int rowsMargins = (out.rows - nRows) / 2;
+		out.adjustROI(-rowsMargins, -rowsMargins, 0, 0);
+	} else {
+		int nCols = int(sizeX / yReduction);
+		int colsMargins = (out.cols - nCols) / 2;
+		out.adjustROI(0, 0, -colsMargins, -colsMargins);
+	}
 
-    // now paint each marker
-    Dictionary &dictionary = *(_board->dictionary);
-    Mat marker;
-    Point2f outCorners[3];
-    Point2f inCorners[3];
-    for(unsigned int m = 0; m < _board->objPoints.size(); m++) {
-        // transform corners to markerZone coordinates
-        for(int j = 0; j < 3; j++) {
-            Point2f pf = Point2f(_board->objPoints[m][j].x, _board->objPoints[m][j].y);
-            // move top left to 0, 0
-            pf -= Point2f(minX, minY);
-            pf.x = pf.x / sizeX * float(out.cols);
-            pf.y = (1.0f - pf.y / sizeY) * float(out.rows);
-            outCorners[j] = pf;
-        }
+	// now paint each marker
+	Dictionary &dictionary = *(_board->dictionary);
+	Mat marker;
+	Point2f outCorners[3];
+	Point2f inCorners[3];
+	for(unsigned int m = 0; m < _board->objPoints.size(); m++) {
+		// transform corners to markerZone coordinates
+		for(int j = 0; j < 3; j++) {
+			Point2f pf = Point2f(_board->objPoints[m][j].x, _board->objPoints[m][j].y);
+			// move top left to 0, 0
+			pf -= Point2f(minX, minY);
+			pf.x = pf.x / sizeX * float(out.cols);
+			pf.y = (1.0f - pf.y / sizeY) * float(out.rows);
+			outCorners[j] = pf;
+		}
 
-        // get marker
-        Size dst_sz(outCorners[2] - outCorners[0]); // assuming CCW order
-        // dst_sz.width = dst_sz.height = std::min(dst_sz.width, dst_sz.height); //marker should be square
+		// get marker
+		Size dst_sz(outCorners[2] - outCorners[0]); // assuming CCW order
+		// dst_sz.width = dst_sz.height = std::min(dst_sz.width, dst_sz.height); //marker should be square
 		double diag = std::round(std::hypot(dst_sz.width, dst_sz.height));
 		int side = std::round(diag / std::sqrt(2));
 

aruco_pose/src/draw.h

@@ -3,4 +3,4 @@
 #include <opencv2/opencv.hpp>
 #include <opencv2/aruco.hpp>
 
-void drawPlanarBoard(cv::aruco::Board *_board, cv::Size outSize, cv::OutputArray _img, int marginSize, int borderBits);
+void _drawPlanarBoard(cv::aruco::Board *_board, cv::Size outSize, cv::OutputArray _img, int marginSize, int borderBits);

aruco_pose/test/basic.py

@@ -22,6 +22,7 @@ class TestArucoPose(unittest.TestCase):
         assert markers.header.frame_id == 'main_camera_optical'
 
         assert markers.markers[0].id == 2
+        assert markers.markers[0].length == approx(0.33)
         assert markers.markers[0].pose.position.x == approx(0.36706567854)
         assert markers.markers[0].pose.position.y == approx(0.290484516644)
         assert markers.markers[0].pose.position.z == approx(2.18787602301)
@@ -39,6 +40,7 @@ class TestArucoPose(unittest.TestCase):
         assert markers.markers[0].c4.y == approx(429.442260742)
 
         assert markers.markers[3].id == 3
+        assert markers.markers[3].length == approx(0.1)
         assert markers.markers[3].pose.position.x == approx(-0.1805169666)
         assert markers.markers[3].pose.position.y == approx(-0.200697302327)
         assert markers.markers[3].pose.position.z == approx(0.585767514823)
@@ -56,10 +58,19 @@ class TestArucoPose(unittest.TestCase):
         assert markers.markers[3].c4.y == approx(143.442276001)
 
         assert markers.markers[1].id == 1
+        assert markers.markers[1].length == approx(0.33)
         assert markers.markers[2].id == 4
+        assert markers.markers[2].length == approx(0.33)
 
     def test_visualization(self):
         vis = rospy.wait_for_message('aruco_detect/visualization', VisMarkerArray, timeout=5)
+        assert len(vis.markers) == 9
+
+    def test_debug(self):
+        img = rospy.wait_for_message('aruco_detect/debug', Image, timeout=5)
+        assert img.width == 640
+        assert img.height == 480
+        assert img.header.frame_id == 'main_camera_optical'
 
     def test_map(self):
         pose = rospy.wait_for_message('aruco_map/pose', PoseWithCovarianceStamped, timeout=5)
@@ -72,6 +83,18 @@ class TestArucoPose(unittest.TestCase):
         assert pose.pose.pose.orientation.z == approx(-0.0300861070302)
         assert pose.pose.pose.orientation.w == approx(0.0482143590507)
 
+    def test_map_image(self):
+        img = rospy.wait_for_message('aruco_map/image', Image, timeout=5)
+        assert img.width == 2000
+        assert img.height == 2000
+        assert img.encoding == 'mono8'
+
+    def test_map_visualization(self):
+        vis = rospy.wait_for_message('aruco_map/visualization', VisMarkerArray, timeout=5)
+
+    def test_map_debug(self):
+        img = rospy.wait_for_message('aruco_map/debug', Image, timeout=5)
+
     # def test_transforms(self):
     #     pass
 

aruco_pose/test/basic.test

@@ -20,7 +20,7 @@
         <remap from="camera_info" to="main_camera/camera_info"/>
         <remap from="markers" to="aruco_detect/markers"/>
         <param name="type" value="map"/>
-        <param name="map" value="$(find aruco_pose)/map/map.txt"/>
+        <param name="map" value="$(find aruco_pose)/test/basic.txt"/>
     </node>
 
     <test test-name="test_aruco_pose" pkg="aruco_pose" type="basic.py"/>

aruco_pose/test/basic.txt

@@ -0,0 +1,5 @@
+1	0.33	0	0	0	0	0	0
+2	0.33	1	0	0	0	0	0
+3	0.33	0	1	0	0	0	0
+4	0.33	1	1	0	0	0	0
+10	0.5	0.5	2	0	1.2	0	0

builder/assets/99-px4fmu.rules

@@ -1,15 +1,15 @@
 # PixHawk (px4fmu-v2), px4fmu-v3
-SUBSYSTEM=="tty", ATTRS{idVendor}=="26ac", ATTRS{idProduct}=="0011", SYMLINK+="px4fmu"
+SUBSYSTEM=="tty", ATTRS{idVendor}=="26ac", ATTRS{idProduct}=="0011", ATTRS{product}=="PX4 FMU v2.x", SYMLINK+="px4fmu"
 # PixRacer (px4fmu-v4)
-SUBSYSTEM=="tty", ATTRS{idVendor}=="26ac", ATTRS{idProduct}=="0012", SYMLINK+="px4fmu"
+SUBSYSTEM=="tty", ATTRS{idVendor}=="26ac", ATTRS{idProduct}=="0012", ATTRS{product}=="PX4 FMU v4.x", SYMLINK+="px4fmu"
 # px4fmu-v5
-SUBSYSTEM=="tty", ATTRS{idVendor}=="26ac", ATTRS{idProduct}=="0012", SYMLINK+="px4fmu"
+SUBSYSTEM=="tty", ATTRS{idVendor}=="26ac", ATTRS{idProduct}=="0032", ATTRS{product}=="PX4 FMU v5.x", SYMLINK+="px4fmu"
 # auav
-SUBSYSTEM=="tty", ATTRS{idVendor}=="26ac", ATTRS{idProduct}=="0021", SYMLINK+="px4fmu"
+SUBSYSTEM=="tty", ATTRS{idVendor}=="26ac", ATTRS{idProduct}=="0021", ATTRS{product}=="PX4 AUAV x2.1", SYMLINK+="px4fmu"
 # crazyflie
-SUBSYSTEM=="tty", ATTRS{idVendor}=="26ac", ATTRS{idProduct}=="0016", SYMLINK+="px4fmu"
+SUBSYSTEM=="tty", ATTRS{idVendor}=="26ac", ATTRS{idProduct}=="0016", ATTRS{product}=="PX4 Crazyflie v2.0", SYMLINK+="px4fmu"
 # px4fmu-v4pro
-SUBSYSTEM=="tty", ATTRS{idVendor}=="26ac", ATTRS{idProduct}=="0013", SYMLINK+="px4fmu"
+SUBSYSTEM=="tty", ATTRS{idVendor}=="26ac", ATTRS{idProduct}=="0013", ATTRS{product}=="PX4 FMU v4.x PRO", SYMLINK+="px4fmu"
 # Omnibus
-SUBSYSTEM=="tty", ATTRS{idVendor}=="26ac", ATTRS{idProduct}=="0001", SYMLINK+="px4fmu"
+SUBSYSTEM=="tty", ATTRS{idVendor}=="26ac", ATTRS{idProduct}=="0001", ATTRS{product}=="PX4 OmnibusF4SD", SYMLINK+="px4fmu"
 

clever/src/simple_offboard.cpp

@@ -454,10 +454,10 @@ inline void checkState()
 		throw std::runtime_error("No connection to FCU, https://clever.copterexpress.com/connection.html");
 }
 
-inline void serve(enum setpoint_type_t sp_type, float x, float y, float z, float vx, float vy, float vz,
-                  float pitch, float roll, float yaw, float pitch_rate, float roll_rate, float yaw_rate,
-                  float lat, float lon, float thrust, float speed, string frame_id, bool auto_arm,
-                  uint8_t& success, string& message)
+bool serve(enum setpoint_type_t sp_type, float x, float y, float z, float vx, float vy, float vz,
+           float pitch, float roll, float yaw, float pitch_rate, float roll_rate, float yaw_rate,
+           float lat, float lon, float thrust, float speed, string frame_id, bool auto_arm,
+           uint8_t& success, string& message)
 {
 	auto stamp = ros::Time::now();
 
@@ -601,42 +601,36 @@ inline void serve(enum setpoint_type_t sp_type, float x, float y, float z, float
 		message = e.what();
 		ROS_INFO("simple_offboard: %s", message.c_str());
 		busy = false;
-		return;
+		return true;
 	}
 
 	success = true;
 	busy = false;
-	return;
+	return true;
 }
 
 bool navigate(Navigate::Request& req, Navigate::Response& res) {
-	serve(NAVIGATE, req.x, req.y, req.z, 0, 0, 0, 0, 0, req.yaw, 0, 0, req.yaw_rate, 0, 0, 0, req.speed, req.frame_id, req.auto_arm, res.success, res.message);
-	return true;
+	return serve(NAVIGATE, req.x, req.y, req.z, 0, 0, 0, 0, 0, req.yaw, 0, 0, req.yaw_rate, 0, 0, 0, req.speed, req.frame_id, req.auto_arm, res.success, res.message);
 }
 
 bool navigateGlobal(NavigateGlobal::Request& req, NavigateGlobal::Response& res) {
-	serve(NAVIGATE_GLOBAL, 0, 0, req.z, 0, 0, 0, 0, 0, req.yaw, 0, 0, req.yaw_rate, req.lat, req.lon, 0, req.speed, req.frame_id, req.auto_arm, res.success, res.message);
-	return true;
+	return serve(NAVIGATE_GLOBAL, 0, 0, req.z, 0, 0, 0, 0, 0, req.yaw, 0, 0, req.yaw_rate, req.lat, req.lon, 0, req.speed, req.frame_id, req.auto_arm, res.success, res.message);
 }
 
 bool setPosition(SetPosition::Request& req, SetPosition::Response& res) {
-	serve(POSITION, req.x, req.y, req.z, 0, 0, 0, 0, 0, req.yaw, 0, 0, req.yaw_rate, 0, 0, 0, 0, req.frame_id, req.auto_arm, res.success, res.message);
-	return true;
+	return serve(POSITION, req.x, req.y, req.z, 0, 0, 0, 0, 0, req.yaw, 0, 0, req.yaw_rate, 0, 0, 0, 0, req.frame_id, req.auto_arm, res.success, res.message);
 }
 
 bool setVelocity(SetVelocity::Request& req, SetVelocity::Response& res) {
-	serve(VELOCITY, 0, 0, 0, req.vx, req.vy, req.vz, 0, 0, req.yaw, 0, 0, req.yaw_rate, 0, 0, 0, 0, req.frame_id, req.auto_arm, res.success, res.message);
-	return true;
+	return serve(VELOCITY, 0, 0, 0, req.vx, req.vy, req.vz, 0, 0, req.yaw, 0, 0, req.yaw_rate, 0, 0, 0, 0, req.frame_id, req.auto_arm, res.success, res.message);
 }
 
 bool setAttitude(SetAttitude::Request& req, SetAttitude::Response& res) {
-	serve(ATTITUDE, 0, 0, 0, 0, 0, 0, req.pitch, req.roll, req.yaw, 0, 0, 0, 0, 0, req.thrust, 0, req.frame_id, req.auto_arm, res.success, res.message);
-	return true;
+	return serve(ATTITUDE, 0, 0, 0, 0, 0, 0, req.pitch, req.roll, req.yaw, 0, 0, 0, 0, 0, req.thrust, 0, req.frame_id, req.auto_arm, res.success, res.message);
 }
 
 bool setRates(SetRates::Request& req, SetRates::Response& res) {
-	serve(RATES, 0, 0, 0, 0, 0, 0, 0, 0, 0, req.pitch_rate, req.roll_rate, req.yaw_rate, 0, 0, req.thrust, 0, "", req.auto_arm, res.success, res.message);
-	return true;
+	return serve(RATES, 0, 0, 0, 0, 0, 0, 0, 0, 0, req.pitch_rate, req.roll_rate, req.yaw_rate, 0, 0, req.thrust, 0, "", req.auto_arm, res.success, res.message);
 }
 
 bool land(std_srvs::Trigger::Request& req, std_srvs::Trigger::Response& res)