diff --git a/mavros/src/plugins/ftp.cpp b/mavros/src/plugins/ftp.cpp index a704462af..4d8ee4bc0 100644 --- a/mavros/src/plugins/ftp.cpp +++ b/mavros/src/plugins/ftp.cpp @@ -567,7 +567,7 @@ class FTPPlugin : public MavRosPlugin { void send_reset() { ROS_DEBUG_NAMED("ftp", "FTP:m: kCmdResetSessions"); - if (session_file_map.size() > 0) { + if (!session_file_map.empty()) { ROS_WARN_NAMED("ftp", "FTP: Reset closes %zu sessons", session_file_map.size()); session_file_map.clear(); @@ -579,7 +579,7 @@ class FTPPlugin : public MavRosPlugin { } /// Send any command with string payload (usually file/dir path) - inline void send_any_path_command(FTPRequest::Opcode op, const std::string debug_msg, std::string &path, uint32_t offset) { + inline void send_any_path_command(FTPRequest::Opcode op, const std::string &debug_msg, std::string &path, uint32_t offset) { ROS_DEBUG_STREAM_NAMED("ftp", "FTP:m: " << debug_msg << path << " off: " << offset); FTPRequest req(op); req.header()->offset = offset; diff --git a/mavros/src/plugins/setpoint_attitude.cpp b/mavros/src/plugins/setpoint_attitude.cpp index 1e092a423..ad426175b 100644 --- a/mavros/src/plugins/setpoint_attitude.cpp +++ b/mavros/src/plugins/setpoint_attitude.cpp @@ -55,8 +55,9 @@ class SetpointAttitudePlugin : public MavRosPlugin, sp_nh.param("tf/rate_limit", tf_rate, 10.0); if (tf_listen) { - ROS_INFO_STREAM_NAMED("attitude", "Listen to desired attitude transform " << tf_frame_id - << " -> " << tf_child_frame_id); + ROS_INFO_STREAM_NAMED("attitude", + "Listen to desired attitude transform " + << tf_frame_id << " -> " << tf_child_frame_id); tf2_start("AttitudeSpTF", &SetpointAttitudePlugin::transform_cb); } else { @@ -116,20 +117,13 @@ class SetpointAttitudePlugin : public MavRosPlugin, const uint8_t ignore_all_except_q = (1 << 6) | (7 << 0); float q[4]; - // Eigen use same convention as mavlink: w x y z - Eigen::Map q_out(q); - q_out = UAS::transform_frame_enu_ned(Eigen::Quaterniond(tr.rotation())).cast(); - - float q_transformed[4]; - - q_transformed[0] = q_out.w(); - q_transformed[1] = q_out.x(); - q_transformed[2] = q_out.y(); - q_transformed[3] = q_out.z(); + UAS::quaternion_to_mavlink( + UAS::transform_frame_enu_ned(Eigen::Quaterniond(tr.rotation())), + q); set_attitude_target(stamp.toNSec() / 1000000, ignore_all_except_q, - q_transformed, + q, 0.0, 0.0, 0.0, 0.0); } diff --git a/mavros/src/plugins/sys_status.cpp b/mavros/src/plugins/sys_status.cpp index b6d8f1b80..87d887885 100644 --- a/mavros/src/plugins/sys_status.cpp +++ b/mavros/src/plugins/sys_status.cpp @@ -32,14 +32,14 @@ namespace mavplugin { class HeartbeatStatus : public diagnostic_updater::DiagnosticTask { public: - HeartbeatStatus(const std::string name, size_t win_size) : + HeartbeatStatus(const std::string &name, size_t win_size) : diagnostic_updater::DiagnosticTask(name), + times_(win_size), + seq_nums_(win_size), window_size_(win_size), min_freq_(0.2), max_freq_(100), tolerance_(0.1), - times_(win_size), - seq_nums_(win_size), autopilot(MAV_AUTOPILOT_GENERIC), type(MAV_TYPE_GENERIC), system_status(MAV_STATE_UNINIT) @@ -52,8 +52,7 @@ class HeartbeatStatus : public diagnostic_updater::DiagnosticTask ros::Time curtime = ros::Time::now(); count_ = 0; - for (int i = 0; i < window_size_; i++) - { + for (size_t i = 0; i < window_size_; i++) { times_[i] = curtime; seq_nums_[i] = count_; } @@ -128,7 +127,7 @@ class HeartbeatStatus : public diagnostic_updater::DiagnosticTask class SystemStatusDiag : public diagnostic_updater::DiagnosticTask { public: - SystemStatusDiag(const std::string name) : + SystemStatusDiag(const std::string &name) : diagnostic_updater::DiagnosticTask(name), last_st {} { }; @@ -203,7 +202,7 @@ class SystemStatusDiag : public diagnostic_updater::DiagnosticTask class BatteryStatusDiag : public diagnostic_updater::DiagnosticTask { public: - BatteryStatusDiag(const std::string name) : + BatteryStatusDiag(const std::string &name) : diagnostic_updater::DiagnosticTask(name), voltage(-1.0), current(0.0), @@ -253,7 +252,7 @@ class BatteryStatusDiag : public diagnostic_updater::DiagnosticTask class MemInfo : public diagnostic_updater::DiagnosticTask { public: - MemInfo(const std::string name) : + MemInfo(const std::string &name) : diagnostic_updater::DiagnosticTask(name), freemem(-1), brkval(0) @@ -291,7 +290,7 @@ class MemInfo : public diagnostic_updater::DiagnosticTask class HwStatus : public diagnostic_updater::DiagnosticTask { public: - HwStatus(const std::string name) : + HwStatus(const std::string &name) : diagnostic_updater::DiagnosticTask(name), vcc(-1.0), i2cerr(0), diff --git a/mavros/src/plugins/sys_time.cpp b/mavros/src/plugins/sys_time.cpp index 32be7c5e7..c57d3cd83 100644 --- a/mavros/src/plugins/sys_time.cpp +++ b/mavros/src/plugins/sys_time.cpp @@ -29,7 +29,7 @@ namespace mavplugin { class TimeSyncStatus : public diagnostic_updater::DiagnosticTask { public: - TimeSyncStatus(const std::string name, size_t win_size) : + TimeSyncStatus(const std::string &name, size_t win_size) : diagnostic_updater::DiagnosticTask(name), window_size_(win_size), min_freq_(0.01), diff --git a/mavros_extras/src/plugins/mocap_pose_estimate.cpp b/mavros_extras/src/plugins/mocap_pose_estimate.cpp index 695d2290f..e6274c286 100644 --- a/mavros_extras/src/plugins/mocap_pose_estimate.cpp +++ b/mavros_extras/src/plugins/mocap_pose_estimate.cpp @@ -91,14 +91,13 @@ class MocapPoseEstimatePlugin : public MavRosPlugin /* -*- mid-level helpers -*- */ void mocap_pose_cb(const geometry_msgs::PoseStamped::ConstPtr &pose) { - float q[4]; - - // Eigen has same order as mavlink: w x y z Eigen::Quaterniond q_enu; - Eigen::Map q_out(q); + float q[4]; tf::quaternionMsgToEigen(pose->pose.orientation, q_enu); - q_out = UAS::transform_frame_enu_ned(q_enu).cast(); + UAS::quaternion_to_mavlink( + UAS::transform_frame_enu_ned(q_enu), + q); auto position = UAS::transform_frame_enu_ned( Eigen::Vector3d( @@ -116,14 +115,13 @@ class MocapPoseEstimatePlugin : public MavRosPlugin /* -*- callbacks -*- */ void mocap_tf_cb(const geometry_msgs::TransformStamped::ConstPtr &trans) { - float q[4]; - - // Eigen has same order as mavlink: w x y z Eigen::Quaterniond q_enu; - Eigen::Map q_out(q); + float q[4]; tf::quaternionMsgToEigen(trans->transform.rotation, q_enu); - q_out = UAS::transform_frame_enu_ned(q_enu).cast(); + UAS::quaternion_to_mavlink( + UAS::transform_frame_enu_ned(q_enu), + q); auto position = UAS::transform_frame_enu_ned( Eigen::Vector3d(