Merge branch 'master' into coverity_scan

* master: (24 commits)
  uncrustify: fix style on frame conversions
  uncrustify: includes
  plugin: sys_status #266: replace period with rate parameter
  plugin: sys_time #266: Replace period with rate parameters
  frame_conversion: last fix patch
  frame_conversions: use inline functions to identify direction of conversion
  changed frame conversion func name; add 3x3 cov matrix frame conversion; general doxygen comment cleanup
  frame_conversions: added covariance frame conversion for full pose 6x6 matrix
  frame_conversions: added frame_conversion specific lib file; applied correct frame conversion between ENU<->NED
  sys_status #300: PX4 place in [0] lest significant byte of git hash.
  sys_status fix #300: fix u8->hex func.
  plugin: waypoint: cosmetics.
  vibration_plugin: changed vibration to Vector3
  vibration_plugin: msg reformulation
  vibration_plugin: first commit
  Changes some frames from world to body conversion for NED to ENU.
  mavsys #293: add --wait option
  mavsys: Fix arguments help
  mavcmd #293: Add --wait option.
  libmavconn: UDP: Do not exit on Network unreachable error.
  ...

libmavconn/src/udp.cpp

@@ -229,7 +229,11 @@ void MAVConnUDP::do_sendto(bool check_tx_state)
 
 void MAVConnUDP::async_sendto_end(error_code error, size_t bytes_transferred)
 {
-	if (error) {
+	if (error == boost::asio::error::network_unreachable) {
+		logWarn(PFXd "sendto: %s, retrying", channel, error.message().c_str());
+		// do not return, try to resend
+	}
+	else if (error) {
 		logError(PFXd "sendto: %s", channel, error.message().c_str());
 		close();
 		return;

mavros/CMakeLists.txt

@@ -154,11 +154,14 @@ include_directories(
 
 
 add_library(mavros
-  src/lib/uas.cpp
+  src/lib/uas_data.cpp
+  src/lib/uas_stringify.cpp
+  src/lib/uas_timesync.cpp
+  src/lib/uas_sensor_orientation.cpp
+  src/lib/uas_frame_conversions.cpp
   src/lib/mavros.cpp
   src/lib/mavlink_diag.cpp
   src/lib/rosconsole_bridge.cpp
-  src/lib/sensor_orientation.cpp
 )
 add_dependencies(mavros
   mavros_generate_messages_cpp

mavros/include/mavros/mavros_uas.h

@@ -16,6 +16,7 @@
 
 #pragma once
 
+#include <array>
 #include <mutex>
 #include <atomic>
 #include <tf/transform_datatypes.h>
@@ -24,7 +25,6 @@
 
 #include <sensor_msgs/NavSatFix.h>
 
-
 namespace mavros {
 /**
  * @brief helper accessor to FCU link interface
@@ -76,6 +76,8 @@ class UAS {
 public:
 	typedef std::lock_guard<std::recursive_mutex> lock_guard;
 	typedef std::unique_lock<std::recursive_mutex> unique_lock;
+	typedef boost::array<double, 9> Covariance3x3;
+	typedef boost::array<double, 36> Covariance6x6;
 
 	UAS();
 	~UAS() {};
@@ -290,6 +292,156 @@ class UAS {
 	 */
 	static std::string str_system_status(enum MAV_STATE st);
 
+	/**
+	 * @brief Function to match the received orientation received by DISTANCE_SENSOR msg
+	 *        and the rotation of the sensor relative to the FCU.
+	 */
+	static tf::Vector3 sensor_orientation_matching(MAV_SENSOR_ORIENTATION orientation);
+
+	static tf::Vector3 transform_frame_xyz(double _x, double _y, double _z);
+	static tf::Quaternion transform_frame_attitude_q(tf::Quaternion qo);
+	static tf::Vector3 transform_frame_attitude_rpy(double _roll, double _pitch, double _yaw);
+	static Covariance6x6 transform_frame_covariance_pose6x6(Covariance6x6 &_covariance);
+	static Covariance3x3 transform_frame_covariance_general3x3(Covariance3x3 &_covariance);
+
+	/**
+	 * @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
+	 */
+	static inline tf::Vector3 transform_frame_enu_ned_xyz(double _x, double _y, double _z){
+		return transform_frame_xyz(_x, _y, _z);
+	}
+
+	/**
+	 * @brief Function to convert general XYZ values from NED to ENU frames
+	 * @param _x: X coordinate value
+	 * @param _y: Y coordinate value
+	 * @param _z: Z coordinate value
+	 */
+	static inline tf::Vector3 transform_frame_ned_enu_xyz(double _x, double _y, double _z){
+		return transform_frame_xyz(_x, _y, _z);
+	}
+
+	/**
+	 * @brief Function to convert attitude quaternion values from ENU to NED frames
+	 * @param qo: tf::Quaternion format
+	 */
+	static inline tf::Quaternion transform_frame_enu_ned_attitude_q(tf::Quaternion qo){
+		return transform_frame_attitude_q(qo);
+	}
+
+	/**
+	 * @brief Function to convert attitude quaternion values from NED to ENU frames
+	 * @param qo: tf::Quaternion format
+	 */
+	static inline tf::Quaternion transform_frame_ned_enu_attitude_q(tf::Quaternion qo){
+		return transform_frame_attitude_q(qo);
+	}
+
+	/**
+	 * @brief Function to convert attitude euler angle values from ENU to NED frames
+	 * @param _roll: Roll value
+	 * @param _pitch: Pitch value
+	 * @param _yaw: Yaw value
+	 */
+	static inline tf::Vector3 transform_frame_enu_ned_attitude_rpy(double _roll, double _pitch, double _yaw){
+		return transform_frame_attitude_rpy(_roll, _pitch, _yaw);
+	}
+
+	/**
+	 * @brief Function to convert attitude euler angle values from NED to ENU frames
+	 * @param _roll: Roll value
+	 * @param _pitch: Pitch value
+	 * @param _yaw: Yaw value
+	 */
+	static inline tf::Vector3 transform_frame_ned_enu_attitude_rpy(double _roll, double _pitch, double _yaw){
+		return transform_frame_attitude_rpy(_roll, _pitch, _yaw);
+	}
+
+	/**
+	 * @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
+	 */
+	static inline Covariance6x6 transform_frame_enu_ned_covariance_pose6x6(Covariance6x6 _covariance){
+		return transform_frame_covariance_pose6x6(_covariance);
+	}
+
+	/**
+	 * @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
+	 */
+	static inline Covariance6x6 transform_frame_ned_enu_covariance_pose6x6(Covariance6x6 _covariance){
+		return transform_frame_covariance_pose6x6(_covariance);
+	}
+
+	/**
+	 * Matrix formats for the above funtions:
+	 *
+	 * Cov_matrix =	| var_x  cov_xy cov_xz cov_xZ cov_xY cov_xX |
+	 *              | cov_yx var_y  cov_yz cov_yZ cov_yY cov_yX |
+	 *              | cov_zx cov_zy var_z  cov_zZ cov_zY cov_zX |
+	 *              | cov_Zx cov_Zy cov_Zz var_Z  cov_ZY cov_ZX |
+	 *              | 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 |
+	 *
+	 * Compute Covariance matrix in another frame: (according to the law of propagation of covariance)
+	 *
+	 *                      C' = R * C * R^T
+	 */
+
+	/**
+	 * @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
+	 */
+	static inline Covariance3x3 transform_frame_covariance_enu_ned_general3x3(Covariance3x3 _covariance){
+		return transform_frame_covariance_general3x3(_covariance);
+	}
+
+	/**
+	 * @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
+	 */
+	static inline Covariance3x3 transform_frame_covariance_ned_enu_general3x3(Covariance3x3 _covariance){
+		return transform_frame_covariance_general3x3(_covariance);
+	}
+
+	/**
+	 * Matrix formats for the above funtions:
+	 *
+	 * Pos_Cov_matrix =	| var_x  cov_xy cov_xz |
+	 *                      | cov_yx var_y  cov_yz |
+	 *                      | cov_zx cov_zy var_z  |
+	 *
+	 * Vel_Cov_matrix =	| var_vx  cov_vxvy cov_vxvz |
+	 *                      | cov_vyvx var_vy  cov_vyvz |
+	 *                      | cov_vzvx cov_vzvy var_vz  |
+	 *
+	 * Att_Cov_matrix =	| var_Z  cov_ZY cov_ZX |
+	 *                      | 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.
+	 *
+	 * 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
+	 */
+
 private:
 	std::recursive_mutex mutex;
 

mavros/include/mavros/setpoint_mixin.h

@@ -22,7 +22,6 @@
 #include <tf/transform_listener.h>
 
 namespace mavplugin {
-
 /**
  * @brief This mixin adds set_position_target_local_ned()
  *
@@ -40,7 +39,7 @@ class SetPositionTargetLocalNEDMixin {
 		UAS *_uas = static_cast<D *>(this)->uas;
 		mavlink_message_t msg;
 		mavlink_msg_set_position_target_local_ned_pack_chan(UAS_PACK_CHAN(_uas), &msg,
-				time_boot_ms, // why it not usec timestamp?
+				time_boot_ms,	// why it not usec timestamp?
 				UAS_PACK_TGT(_uas),
 				coordinate_frame,
 				type_mask,
@@ -71,7 +70,7 @@ class TFListenerMixin {
 	 * @param _thd_name  listener thread name
 	 * @param cbp        plugin callback function
 	 */
-	void tf_start(const char *_thd_name, void (D::*cbp)(const tf::Transform&, const ros::Time&) ) {
+	void tf_start(const char *_thd_name, void (D::*cbp)(const tf::Transform &, const ros::Time &) ) {
 		thd_name = _thd_name;
 		tf_transform_cb = boost::bind(cbp, static_cast<D *>(this), _1, _2);
 
@@ -102,5 +101,4 @@ class TFListenerMixin {
 		}
 	}
 };
-
-}; // namespace mavplugin
+};	// namespace mavplugin

mavros/include/mavros/utils.h

@@ -17,14 +17,13 @@
 
 #pragma once
 
-#include <array>
 #include <algorithm>
 #include <mavconn/thread_utils.h>
-#include <tf/transform_datatypes.h>
 
 #include <mavros/Mavlink.h>
 #include <mavconn/mavlink_dialect.h>
 
+
 namespace mavutils {
 /**
  * @brief Copy mavros/Mavlink.msg message data to mavlink_message_t
@@ -56,10 +55,4 @@ inline void copy_mavlink_to_ros(const mavlink_message_t *mmsg, mavros::MavlinkPt
 	for (size_t i = 0; i < (mmsg->len + 7) / 8; i++)
 		rmsg->payload64.push_back(mmsg->payload64[i]);
 };
-
-/**
- * @brief Function to match the received orientation received by DISTANCE_SENSOR msg
- * and the rotation of the sensor relative to the FCU.
- */
-tf::Vector3 orientation_matching(uint8_t orientation);
 };	// namespace mavutils

mavros/launch/apm_pluginlists.yaml

@@ -11,6 +11,7 @@ plugin_blacklist:
 - 'vision_*'
 - visualization
 - distance_sensor
+- vibration
 
 plugin_whitelist: []
 #- 'sys_*'

mavros/launch/px4_config.yaml

@@ -171,4 +171,8 @@ visualization:
   child_frame_id: "fcu"
   marker_scale: 2.0
 
+# vibration
+vibration:
+  frame_id: "vibration"
+
 # vim:ts=2 sw=2 et:

mavros/launch/px4_pluginlists.yaml

@@ -1,5 +1,6 @@
 plugin_blacklist:
 - 'image_pub'
+- 'vibration'
 
 plugin_whitelist: []
 #- 'sys_*'

mavros/scripts/mavcmd

@@ -191,6 +191,7 @@ def main():
     parser = argparse.ArgumentParser(description="Commad line tool for sending commands to MAVLink device.")
     parser.add_argument('-n', '--mavros-ns', help="ROS node namespace", default="/mavros")
     parser.add_argument('-v', '--verbose', action='store_true', help="verbose output")
+    parser.add_argument('--wait', action='store_true', help="Wait for establishing FCU connection")
     subarg = parser.add_subparsers()
 
     long_args = subarg.add_parser('long', help="Send any command (COMMAND_LONG)")
@@ -267,6 +268,10 @@ def main():
 
     rospy.init_node("mavcmd", anonymous=True)
     mavros.set_namespace(args.mavros_ns)
+
+    if args.wait:
+        wait_fcu_connection()
+
     command.setup_services()
     args.func(args)
 

mavros/scripts/mavsys

@@ -83,6 +83,7 @@ def main():
     parser = argparse.ArgumentParser(description="Chnage mode and rate on MAVLink device.")
     parser.add_argument('-n', '--mavros-ns', help="ROS node namespace", default="/mavros")
     parser.add_argument('-v', '--verbose', action='store_true', help="verbose output")
+    parser.add_argument('--wait', action='store_true', help="Wait for establishing FCU connection")
     subarg = parser.add_subparsers()
 
     mode_args = subarg.add_parser('mode', help="Set mode")
@@ -100,14 +101,18 @@ def main():
     rate_args.add_argument('--raw-controller', type=int, metavar='rate', help="raw conptoller stream")
     rate_args.add_argument('--position', type=int, metavar='rate', help="position stream")
     rate_args.add_argument('--extra1', type=int, metavar='rate', help="Extra 1 stream")
-    rate_args.add_argument('--extra2', type=int, metavar='rate', help="Extra 1 stream")
-    rate_args.add_argument('--extra3', type=int, metavar='rate', help="Extra 1 stream")
+    rate_args.add_argument('--extra2', type=int, metavar='rate', help="Extra 2 stream")
+    rate_args.add_argument('--extra3', type=int, metavar='rate', help="Extra 3 stream")
     rate_args.add_argument('--stream-id', type=int, nargs=2, metavar=('id', 'rate'), help="any stream")
 
     args = parser.parse_args(rospy.myargv(argv=sys.argv)[1:])
 
     rospy.init_node("mavsys", anonymous=True)
     mavros.set_namespace(args.mavros_ns)
+
+    if args.wait:
+        wait_fcu_connection()
+
     args.func(args)