Merge branch 'master' into coverity_scan

* master: plugin: sys_time, sys_status #266: check that rate is zero test #321: disable tests for broken transforms. lib #321: frame transform are broken. again! revert old math. unittest: added 6x6 Covariance conversion test frame_conversions: update comments; filter covariance by value of element 0 unittests: corrected outputs from conversion tests test: other quaternion transform tests test: UAS::transform_frame_attitude_q() test: test for UAS::transform_frame_attitude_rpy() (ERRORs!) test: test for UAS::transform_frame_xyz() test: Initial import test_frame_conv
mavlink · Jul 1, 2015 · 8f0fe9c · 8f0fe9c
2 parents 59efa87 + 1491ec8
commit 8f0fe9c
Show file tree

Hide file tree

Showing 10 changed files with 322 additions and 56 deletions.
diff --git a/mavros/CMakeLists.txt b/mavros/CMakeLists.txt
@@ -11,6 +11,7 @@ find_package(catkin REQUIRED COMPONENTS
   message_runtime
   pluginlib
   roscpp
+  geometry_msgs
   sensor_msgs
   std_msgs
   std_srvs
@@ -133,8 +134,8 @@ generate_messages(
 catkin_package(
   INCLUDE_DIRS include
   LIBRARIES mavros
-  CATKIN_DEPENDS diagnostic_msgs diagnostic_updater pluginlib roscpp sensor_msgs std_msgs tf geometry_msgs libmavconn
-  DEPENDS Boost mavlink
+  CATKIN_DEPENDS diagnostic_msgs diagnostic_updater pluginlib roscpp sensor_msgs std_msgs tf geometry_msgs libmavconn message_runtime
+  DEPENDS Boost
 )
 
 ###########
@@ -269,13 +270,14 @@ install(DIRECTORY launch/
 ## Testing ##
 #############
 
-## Add gtest based cpp test target and link libraries
-#catkin_add_gtest(mavconn-test test/test_mavconn.cpp)
-#if(TARGET mavconn-test)
-#  target_link_libraries(mavconn-test mavconn)
-#endif()
+if(CATKIN_ENABLE_TESTING)
+
+catkin_add_gtest(libmavros-frame-test test/test_frame_conv.cpp)
+target_link_libraries(libmavros-frame-test mavros)
 
 ## Add folders to be run by python nosetests
 # catkin_add_nosetests(test)
 
+endif()
+
 # vim: ts=2 sw=2 et:
diff --git a/mavros/include/mavros/mavros_uas.h b/mavros/include/mavros/mavros_uas.h
@@ -306,9 +306,10 @@ class UAS {
 
 	/**
 	 * @brief Function to convert general XYZ values from ENU to NED frames
-	 * @param _x: X coordinate value
-	 * @param _y: Y coordinate value
-	 * @param _z: Z coordinate value
+	 * @param _x: X coordinate/direction value
+	 * @param _y: Y coordinate/direction value
+	 * @param _z: Z coordinate/direction value
+	 * @return Translated XYZ values in NED frame
 	 */
 	static inline tf::Vector3 transform_frame_enu_ned_xyz(double _x, double _y, double _z){
 		return transform_frame_xyz(_x, _y, _z);
@@ -319,6 +320,7 @@ class UAS {
 	 * @param _x: X coordinate value
 	 * @param _y: Y coordinate value
 	 * @param _z: Z coordinate value
+	 * @return Translated XYZ values in ENU frame
 	 */
 	static inline tf::Vector3 transform_frame_ned_enu_xyz(double _x, double _y, double _z){
 		return transform_frame_xyz(_x, _y, _z);
@@ -327,6 +329,7 @@ class UAS {
 	/**
 	 * @brief Function to convert attitude quaternion values from ENU to NED frames
 	 * @param qo: tf::Quaternion format
+	 * @return Rotated Quaternion values in NED frame
 	 */
 	static inline tf::Quaternion transform_frame_enu_ned_attitude_q(tf::Quaternion qo){
 		return transform_frame_attitude_q(qo);
@@ -335,6 +338,7 @@ class UAS {
 	/**
 	 * @brief Function to convert attitude quaternion values from NED to ENU frames
 	 * @param qo: tf::Quaternion format
+	 * @return Rotated Quaternion values in ENU frame
 	 */
 	static inline tf::Quaternion transform_frame_ned_enu_attitude_q(tf::Quaternion qo){
 		return transform_frame_attitude_q(qo);
@@ -345,6 +349,7 @@ class UAS {
 	 * @param _roll: Roll value
 	 * @param _pitch: Pitch value
 	 * @param _yaw: Yaw value
+	 * @return Rotated RPY angles values in NED frame
 	 */
 	static inline tf::Vector3 transform_frame_enu_ned_attitude_rpy(double _roll, double _pitch, double _yaw){
 		return transform_frame_attitude_rpy(_roll, _pitch, _yaw);
@@ -355,6 +360,7 @@ class UAS {
 	 * @param _roll: Roll value
 	 * @param _pitch: Pitch value
 	 * @param _yaw: Yaw value
+	 * @return Rotated RPY angles values in ENU frame
 	 */
 	static inline tf::Vector3 transform_frame_ned_enu_attitude_rpy(double _roll, double _pitch, double _yaw){
 		return transform_frame_attitude_rpy(_roll, _pitch, _yaw);
@@ -364,6 +370,7 @@ class UAS {
 	 * @brief Function to convert full 6D pose covariance matrix values from ENU to NED frames
 	 * @details Full 6D pose covariance matrix format: a 3D position plus three attitude angles: roll, pitch and yaw.
 	 * @param _covariance: 6x6 double precision covariance matrix
+	 * @return Propagated 6x6 covariance matrix in NED frame, if _covariance[0] != -1
 	 */
 	static inline Covariance6x6 transform_frame_enu_ned_covariance_pose6x6(Covariance6x6 _covariance){
 		return transform_frame_covariance_pose6x6(_covariance);
@@ -373,6 +380,7 @@ class UAS {
 	 * @brief Function to convert full 6D pose covariance matrix values from NED to ENU frames
 	 * @details Full 6D pose covariance matrix format: a 3D position plus three attitude angles: roll, pitch and yaw.
 	 * @param _covariance: 6x6 double precision covariance matrix
+	 * @return Propagated 6x6 covariance matrix in ENU frame, if _covariance[0] != -1
 	 */
 	static inline Covariance6x6 transform_frame_ned_enu_covariance_pose6x6(Covariance6x6 _covariance){
 		return transform_frame_covariance_pose6x6(_covariance);
@@ -388,12 +396,12 @@ class UAS {
 	 *              | cov_Yx cov_Yy cov_Yz cov_YZ var_Y  cov_YX |
 	 *              | cov_Xx cov_Xy cov_Xz cov_XZ cov_XY var_X  |
 	 *
-	 * Rot_matrix = | 1	 0	 0	 0	 0	 0 |
-	 *              | 0	-1       0	 0	 0	 0 |
-	 *              | 0	 0	-1	 0	 0	 0 |
-	 *              | 0	 0	 0	 1	 0	 0 |
-	 *              | 0	 0	 0	 0	-1	 0 |
-	 *              | 0	 0	 0	 0	 0	-1 |
+	 * Transf_matrix = | 1	 0	 0	 0	 0	 0 |
+	 *                 | 0	-1       0	 0	 0	 0 |
+	 *                 | 0	 0	-1	 0	 0	 0 |
+	 *                 | 0	 0	 0	 1	 0	 0 |
+	 *                 | 0	 0	 0	 0	-1	 0 |
+	 *                 | 0	 0	 0	 0	 0	-1 |
 	 *
 	 * Compute Covariance matrix in another frame: (according to the law of propagation of covariance)
 	 *
@@ -403,6 +411,7 @@ class UAS {
 	/**
 	 * @brief Function to convert position, linear acceleration, angular velocity or attitude RPY covariance matrix values from ENU to NED frames
 	 * @param _covariance: 3x3 double precision covariance matrix
+	 * @return Propagated 3x3 covariance matrix in NED frame, if _covariance[0] != -1
 	 */
 	static inline Covariance3x3 transform_frame_covariance_enu_ned_general3x3(Covariance3x3 _covariance){
 		return transform_frame_covariance_general3x3(_covariance);
@@ -411,6 +420,7 @@ class UAS {
 	/**
 	 * @brief Function to convert position, linear acceleration, angular velocity or attitude RPY covariance matrix values from NED to ENU frames
 	 * @param _covariance: 3x3 double precision covariance matrix
+	 * @return Propagated 3x3 covariance matrix in ENU frame, if _covariance[0] != -1
 	 */
 	static inline Covariance3x3 transform_frame_covariance_ned_enu_general3x3(Covariance3x3 _covariance){
 		return transform_frame_covariance_general3x3(_covariance);
@@ -431,15 +441,15 @@ class UAS {
 	 *                      | cov_YZ var_Y  cov_YX |
 	 *                      | cov_XZ cov_XY var_X  |
 	 *
-	 * Note that for ROS<->ENU frame transformations, the rotation matrix is the same for position and attitude.
+	 * Note that for ROS<->ENU frame transformations, the transformation matrix is the same for position and attitude.
 	 *
 	 * Rot_matrix = | 1	 0	 0 |
 	 *              | 0	-1       0 |
 	 *              | 0	 0	-1 |
 	 *
 	 * Compute Covariance matrix in another frame: (according to the law of propagation of covariance)
 	 *
-	 *                      C' = R * C * R^T
+	 *                      C' = T * C * T^t
 	 */
 
 private:

diff --git a/mavros/launch/apm_config.yaml b/mavros/launch/apm_config.yaml
@@ -7,10 +7,10 @@ startup_px4_usb_quirk: false
 
 # sys_status & sys_time connection options
 conn:
-  heartbeat: 5.0    # send hertbeat every n seconds
-  timeout: 10.0     # hertbeat timeout in seconds
-  timesync: 0.0     # TIMESYNC period in seconds (feature disabled if 0.0)
-  system_time: 0.0  # send system time to FCU every n seconds
+  heartbeat_rate: 1.0    # send hertbeat rate in Hertz
+  timeout: 10.0          # hertbeat timeout in seconds
+  timesync_rate: 0.0     # TIMESYNC rate in Hertz (feature disabled if 0.0)
+  system_time_rate: 0.0  # send system time to FCU rate in Hertz (disabled if 0.0)
 
 # sys_status
 sys:
@@ -121,6 +121,7 @@ mocap:
   use_tf: false   # ~mocap/tf
   use_pose: true  # ~mocap/pose
 
+# px4flow
 px4flow:
   frame_id: "px4flow"
   ranger_fov: !degrees 0.0  # XXX TODO
@@ -145,4 +146,8 @@ visualization:
   child_frame_id: "fcu"
   marker_scale: 2.0
 
+# vibration
+vibration:
+  frame_id: "vibration"
+
 # vim:ts=2 sw=2 et:
diff --git a/mavros/launch/px4_config.yaml b/mavros/launch/px4_config.yaml
@@ -7,10 +7,10 @@ startup_px4_usb_quirk: true
 
 # sys_status & sys_time connection options
 conn:
-  heartbeat: 5.0    # send hertbeat every n seconds
-  timeout: 10.0     # hertbeat timeout in seconds
-  timesync: 0.1     # TIMESYNC period in seconds (feature disabled if 0.0)
-  system_time: 1.0  # send system time to FCU every n seconds
+  heartbeat_rate: 1.0    # send hertbeat rate in Hertz
+  timeout: 10.0          # hertbeat timeout in seconds
+  timesync_rate: 10.0    # TIMESYNC rate in Hertz (feature disabled if 0.0)
+  system_time_rate: 1.0  # send system time to FCU rate in Hertz (disabled if 0.0)
 
 # sys_status
 sys:
@@ -111,7 +111,7 @@ mission:
 
 # --- mavros extras plugins (same order) ---
 
-# distance_sensor
+# distance_sensor (PX4 only)
 distance_sensor:
   hrlv_ez4_pub:
     id: 0

diff --git a/mavros/launch/px4_pluginlists.yaml b/mavros/launch/px4_pluginlists.yaml
@@ -1,6 +1,10 @@
 plugin_blacklist:
-- 'image_pub'
-- 'vibration'
+# common
+- safety_area
+# extras
+- image_pub
+- vibration
+- distance_sensor
 
 plugin_whitelist: []
 #- 'sys_*'
diff --git a/mavros/package.xml b/mavros/package.xml
@@ -23,6 +23,7 @@
   <depend>diagnostic_msgs</depend>
   <depend>diagnostic_updater</depend>
   <depend>message_generation</depend>
+  <depend>message_runtime</depend>
   <depend>pluginlib</depend>
   <depend>roscpp</depend>
   <depend>rospy</depend>
@@ -37,6 +38,8 @@
   <depend>rosconsole_bridge</depend>
   <exec_depend>python-argparse</exec_depend>
 
+  <test_depend>gtest</test_depend>
+
   <export>
     <mavros plugin="${prefix}/mavros_plugins.xml" />
     <rosdoc config="rosdoc.yaml" />

diff --git a/mavros/src/lib/uas_frame_conversions.cpp b/mavros/src/lib/uas_frame_conversions.cpp
@@ -17,8 +17,6 @@
 #include <mavros/mavros_uas.h>
 #include <boost/math/constants/constants.hpp>
 
-const double PI = boost::math::constants::pi<double>();
-
 using namespace mavros;
 
 
@@ -32,18 +30,30 @@ tf::Vector3 UAS::transform_frame_xyz(double _x, double _y, double _z)
 
 tf::Quaternion UAS::transform_frame_attitude_q(tf::Quaternion qo)
 {
-	double roll = PI, pitch = 0.0, yaw = 0.0f;
+	// XXX temporary
+	// same reason as for rpy: return old math.
+#if 0
+	double roll = M_PI, pitch = 0.0, yaw = 0.0;
 	tf::Quaternion qr = tf::createQuaternionFromRPY(roll, pitch, yaw);
 	tf::Quaternion qt = qo * qr;
 	return qt;
+#endif
+	return tf::Quaternion(qo.x(), -qo.y(), -qo.z(), qo.w());
 }
 
 tf::Vector3 UAS::transform_frame_attitude_rpy(double _roll, double _pitch, double _yaw)
 {
-	double roll = _roll + PI;
+	// XXX temporary comment-out new method.
+	// hand-test in rviz + imu plugin show wrong rotation direction on yaw.
+	// APM Planner and MAVProxy shows CW, RVIZ CCW => mavros bug.
+	// return old math.
+#if 0
+	double roll = _roll + M_PI;
 	double pitch = _pitch;
 	double yaw = _yaw;
 	return tf::Vector3(roll, pitch, yaw);
+#endif
+	return transform_frame_xyz(_roll, _pitch, _yaw);
 }
 
 UAS::Covariance6x6 UAS::transform_frame_covariance_pose6x6(UAS::Covariance6x6 &_covariance)
@@ -58,14 +68,25 @@ UAS::Covariance6x6 UAS::transform_frame_covariance_pose6x6(UAS::Covariance6x6 &_
 	};
 
 	UAS::Covariance6x6 covariance;
-	UAS::Covariance6x6 temp;		// temporary matrix = R * C
-
-	// The rotation matrix in this case is a diagonal matrix so R = R^T
-
-	std::transform(rotation.begin(), rotation.end(), _covariance.begin(), temp.begin(), std::multiplies<double>());
-	std::transform(temp.begin(), temp.end(), rotation.begin(), covariance.begin(), std::multiplies<double>());
-
-	return covariance;
+	UAS::Covariance6x6 temp;		// temporary matrix = T * C
+
+	// The transformation matrix in this case is a orthogonal matrix so T = T^t
+
+	/**
+	 * @note According to ROS convention, if one has no estimate for one of the data elements,
+	 * element 0 of the associated covariance matrix to is -1; So no transformation has be applied,
+	 * as the covariance is invalid/unknown; so, it returns the same cov matrix without transformation.
+	 */
+	if (_covariance.at(0) != -1) {
+		// XXX this doesn't multiply matrices correctly. We need Eigen on code!
+		std::transform(rotation.begin(), rotation.end(), _covariance.begin(), temp.begin(), std::multiplies<double>());
+		std::transform(temp.begin(), temp.end(), rotation.begin(), covariance.begin(), std::multiplies<double>());
+		return covariance;
+	}
+	else {
+		_covariance.at(0) = -1;
+		return _covariance;
+	}
 }
 
 UAS::Covariance3x3 UAS::transform_frame_covariance_general3x3(UAS::Covariance3x3 &_covariance)
@@ -77,12 +98,22 @@ UAS::Covariance3x3 UAS::transform_frame_covariance_general3x3(UAS::Covariance3x3
 	};
 
 	UAS::Covariance3x3 covariance;
-	UAS::Covariance3x3 temp;		// temporary matrix = R * C
-
-	// The rotation matrix in this case is a diagonal matrix so R = R^T
-
-	std::transform(rotation.begin(), rotation.end(), _covariance.begin(), temp.begin(), std::multiplies<double>());
-	std::transform(temp.begin(), temp.end(), rotation.begin(), covariance.begin(), std::multiplies<double>());
-
-	return covariance;
+	UAS::Covariance3x3 temp;		// temporary matrix = T * C
+
+	// The transformation matrix in this case is a orthogonal matrix so T = T^t
+
+	/**
+	 * @note According to ROS convention, if one has no estimate for one of the data elements,
+	 * element 0 of the associated covariance matrix to is -1; So no transformation has be applied,
+	 * as the covariance is invalid/unknown; so, it returns the same cov matrix without transformation.
+	 */
+	if (_covariance.at(0) != -1) {
+		// XXX this doesn't multiply matrices correctly. We need Eigen on code!
+		std::transform(rotation.begin(), rotation.end(), _covariance.begin(), temp.begin(), std::multiplies<double>());
+		std::transform(temp.begin(), temp.end(), rotation.begin(), covariance.begin(), std::multiplies<double>());
+		return covariance;
+	}
+	else {
+		return _covariance;
+	}
 }
diff --git a/mavros/src/plugins/sys_status.cpp b/mavros/src/plugins/sys_status.cpp
@@ -361,12 +361,11 @@ class SystemStatusPlugin : public MavRosPlugin
 		double min_voltage;
 
 		nh.param("conn/timeout", conn_timeout_d, 30.0);
-		nh.param("conn/heartbeat", conn_heartbeat_d, 0.0);
 		nh.param("sys/min_voltage", min_voltage, 6.0);
 		nh.param("sys/disable_diag", disable_diag, false);
 
 		// rate parameter
-		if (nh.getParam("conn/heartbeat_rate", conn_heartbeat_d)) {
+		if (nh.getParam("conn/heartbeat_rate", conn_heartbeat_d) && conn_heartbeat_d != 0.0) {
 			conn_heartbeat = ros::Duration(ros::Rate(conn_heartbeat_d));
 		}
 		else if (nh.getParam("conn/heartbeat", conn_heartbeat_d)) {

diff --git a/mavros/src/plugins/sys_time.cpp b/mavros/src/plugins/sys_time.cpp
@@ -146,10 +146,7 @@ class SystemTimePlugin : public MavRosPlugin {
 
 		uas = &uas_;
 
-		// deprecated params
-		nh.param("conn/timesync", conn_timesync_d, 0.0);
-
-		if (nh.getParam("conn/system_time_rate", conn_system_time_d)) {
+		if (nh.getParam("conn/system_time_rate", conn_system_time_d) && conn_system_time_d != 0.0) {
 			conn_system_time = ros::Duration(ros::Rate(conn_system_time_d));
 		}
 		else if (nh.getParam("conn/system_time", conn_system_time_d)) {
@@ -159,7 +156,7 @@ class SystemTimePlugin : public MavRosPlugin {
 			conn_system_time = ros::Duration(conn_system_time_d);
 		}
 
-		if (nh.getParam("conn/timesync_rate", conn_timesync_d)) {
+		if (nh.getParam("conn/timesync_rate", conn_timesync_d) && conn_timesync_d != 0.0) {
 			conn_timesync = ros::Duration(ros::Rate(conn_timesync_d));
 		}
 		else if (nh.getParam("conn/timesync", conn_timesync_d)) {