Merge pull request #1744 from amilcarlucas/pr_gimbal_diagnostics_fixes

mount_control.cpp: detect MOUNT_ORIENTATION stale messages

mavros/launch/apm_config.yaml

@@ -213,6 +213,9 @@ mocap:
 mount:
  debounce_s: 4.0
  err_threshold_deg: 10.0
+ negate_measured_roll: false
+ negate_measured_pitch: false
+ negate_measured_yaw: false
 
 # odom
 odometry:

mavros/launch/px4_config.yaml

@@ -220,6 +220,14 @@ mocap:
  use_tf: false # ~mocap/tf
  use_pose: true # ~mocap/pose
 
+# mount_control
+mount:
+ debounce_s: 4.0
+ err_threshold_deg: 10.0
+ negate_measured_roll: false
+ negate_measured_pitch: false
+ negate_measured_yaw: false
+
 # odom
 odometry:
  fcu:

mavros_extras/src/plugins/mount_control.cpp

@@ -39,16 +39,17 @@ class MountStatusDiag : public diagnostic_updater::DiagnosticTask
 public:
  MountStatusDiag(const std::string &name) :
  diagnostic_updater::DiagnosticTask(name),
+ _last_orientation_update(0, 0),
+ _debounce_s(NAN),
  _roll_deg(NAN),
  _pitch_deg(NAN),
  _yaw_deg(NAN),
  _setpoint_roll_deg(NAN),
  _setpoint_pitch_deg(NAN),
  _setpoint_yaw_deg(NAN),
- _error_detected(false),
  _err_threshold_deg(NAN),
- _mode(255),
- _debounce_s(NAN)
+ _error_detected(false),
+ _mode(255)
  { }
 
  void set_err_threshold_deg(float threshold_deg) {
@@ -61,11 +62,12 @@ class MountStatusDiag : public diagnostic_updater::DiagnosticTask
  _debounce_s = debounce_s;
  }
 
- void set_status(float roll_deg, float pitch_deg, float yaw_deg) {
+ void set_status(float roll_deg, float pitch_deg, float yaw_deg, ros::Time timestamp) {
  std::lock_guard<std::mutex> lock(mutex);
  _roll_deg = roll_deg;
  _pitch_deg = pitch_deg;
  _yaw_deg = yaw_deg;
+ _last_orientation_update = timestamp;
  }
 
  void set_setpoint(float roll_deg, float pitch_deg, float yaw_deg, uint8_t mode) {
@@ -82,6 +84,7 @@ class MountStatusDiag : public diagnostic_updater::DiagnosticTask
  float pitch_err_deg;
  float yaw_err_deg;
  bool error_detected = false;
+ bool stale = false;
 
  if (_mode != mavros_msgs::MountControl::MAV_MOUNT_MODE_MAVLINK_TARGETING) {
  // Can only directly compare the MAV_CMD_DO_MOUNT_CONTROL angles with the MOUNT_ORIENTATION angles when in MAVLINK_TARGETING mode
@@ -90,6 +93,7 @@ class MountStatusDiag : public diagnostic_updater::DiagnosticTask
  return;
  }
 
+ const ros::Time now = ros::Time::now();
  {
  std::lock_guard<std::mutex> lock(mutex);
  roll_err_deg = _setpoint_roll_deg - _roll_deg;
@@ -106,12 +110,14 @@ class MountStatusDiag : public diagnostic_updater::DiagnosticTask
  if (fabs(yaw_err_deg) > _err_threshold_deg) {
  error_detected = true;
  }
+ if (now - _last_orientation_update > ros::Duration(5, 0)) {
+ stale = true;
+ }
  // accessing the _debounce_s variable should be done inside this mutex,
  // but we can treat it as an atomic variable, and save the trouble
  }
 
  // detect error state changes
- const ros::Time now = ros::Time::now();
  if (!_error_detected && error_detected) {
  _error_started = now;
  _error_detected = true;
@@ -121,7 +127,9 @@ class MountStatusDiag : public diagnostic_updater::DiagnosticTask
  }
 
  // debounce errors
- if (_error_detected && (now - _error_started > ros::Duration(_debounce_s))) {
+ if (stale) {
+ stat.summary(diagnostic_msgs::DiagnosticStatus::STALE, "No MOUNT_ORIENTATION received in the last 5 s");
+ } else if (_error_detected && (now - _error_started > ros::Duration(_debounce_s))) {
  stat.summary(diagnostic_msgs::DiagnosticStatus::ERROR, "angle error too high");
  } else {
  stat.summary(diagnostic_msgs::DiagnosticStatus::OK, "Normal");
@@ -135,6 +143,7 @@ class MountStatusDiag : public diagnostic_updater::DiagnosticTask
 private:
  std::mutex mutex;
  ros::Time _error_started;
+ ros::Time _last_orientation_update;
  double _debounce_s;
  float _roll_deg;
  float _pitch_deg;
@@ -170,12 +179,33 @@ class MountControlPlugin : public plugin::PluginBase {
  mount_status_pub = mount_nh.advertise<geometry_msgs::Vector3Stamped>("status", 10);
  configure_srv = mount_nh.advertiseService("configure", &MountControlPlugin::mount_configure_cb, this);
 
+ // some gimbals send negated/inverted angle measurements
+ // these parameters correct that to obey the MAVLink frame convention
+ mount_nh.param<bool>("negate_measured_roll", negate_measured_roll, false);
+ mount_nh.param<bool>("negate_measured_pitch", negate_measured_pitch, false);
+ mount_nh.param<bool>("negate_measured_yaw", negate_measured_yaw, false);
+ if (!mount_nh.getParam("negate_measured_roll", negate_measured_roll)) {
+ ROS_WARN("Could not retrive negate_measured_roll parameter value, using default (%d)", negate_measured_roll);
+ }
+ if (!mount_nh.getParam("negate_measured_pitch", negate_measured_pitch)) {
+ ROS_WARN("Could not retrive negate_measured_pitch parameter value, using default (%d)", negate_measured_pitch);
+ }
+ if (!mount_nh.getParam("negate_measured_yaw", negate_measured_yaw)) {
+ ROS_WARN("Could not retrive negate_measured_yaw parameter value, using default (%d)", negate_measured_yaw);
+ }
+
  bool disable_diag;
  if (nh.getParam("sys/disable_diag", disable_diag) && !disable_diag) {
  double debounce_s;
  double err_threshold_deg;
  mount_nh.param("debounce_s", debounce_s, 4.0);
  mount_nh.param("err_threshold_deg", err_threshold_deg, 10.0);
+ if (!mount_nh.getParam("debounce_s", debounce_s)) {
+ ROS_WARN("Could not retrive debounce_s parameter value, using default (%f)", debounce_s);
+ }
+ if (!mount_nh.getParam("err_threshold_deg", err_threshold_deg)) {
+ ROS_WARN("Could not retrive err_threshold_deg parameter value, using default (%f)", err_threshold_deg);
+ }
  mount_diag.set_debounce_s(debounce_s);
  mount_diag.set_err_threshold_deg(err_threshold_deg);
  UAS_DIAG(m_uas).add(mount_diag);
@@ -199,6 +229,9 @@ class MountControlPlugin : public plugin::PluginBase {
  ros::ServiceServer configure_srv;
 
  MountStatusDiag mount_diag;
+ bool negate_measured_roll;
+ bool negate_measured_pitch;
+ bool negate_measured_yaw;
 
  /**
  * @brief Publish the mount orientation
@@ -209,13 +242,24 @@ class MountControlPlugin : public plugin::PluginBase {
  */
  void handle_mount_orientation(const mavlink::mavlink_message_t *msg, mavlink::common::msg::MOUNT_ORIENTATION &mo)
  {
- auto q = ftf::quaternion_from_rpy(Eigen::Vector3d(mo.roll, mo.pitch, mo.yaw) * M_PI / 180.0);
+ const auto timestamp = ros::Time::now();
+ // some gimbals send negated/inverted angle measurements, correct that to obey the MAVLink frame convention
+ if (negate_measured_roll) {
+ mo.roll = -mo.roll;
+ }
+ if (negate_measured_pitch) {
+ mo.pitch = -mo.pitch;
+ }
+ if (negate_measured_yaw) {
+ mo.yaw = -mo.yaw;
+ mo.yaw_absolute = -mo.yaw_absolute;
+ }
+ const auto q = ftf::quaternion_from_rpy(Eigen::Vector3d(mo.roll, mo.pitch, mo.yaw) * M_PI / 180.0);
  geometry_msgs::Quaternion quaternion_msg;
  tf::quaternionEigenToMsg(q, quaternion_msg);
  mount_orientation_pub.publish(quaternion_msg);
 
- // pitch angle is inverted to fix a gremsy implementation bug
- mount_diag.set_status(mo.roll, -mo.pitch, mo.yaw_absolute);
+ mount_diag.set_status(mo.roll, mo.pitch, mo.yaw_absolute, timestamp);
  }
 
  /**
@@ -267,7 +311,7 @@ class MountControlPlugin : public plugin::PluginBase {
 
  UAS_FCU(m_uas)->send_message_ignore_drop(cmd);
 
- mount_diag.set_setpoint(req->roll/100.0f, req->pitch/100.0f, req->yaw/100.0f, req->mode);
+ mount_diag.set_setpoint(req->roll*0.01f, req->pitch*0.01f, req->yaw*0.01f, req->mode);
  }
 
  bool mount_configure_cb(mavros_msgs::MountConfigure::Request &req,
@@ -292,7 +336,8 @@ class MountControlPlugin : public plugin::PluginBase {
  cmd.request.param7 = req.yaw_input;
 
  ROS_DEBUG_NAMED("mount", "MountConfigure: Request mode %u ", req.mode);
- res.success = client.call(cmd);
+ client.call(cmd);
+ res.success = cmd.response.success;
  }
  catch (ros::InvalidNameException &ex) {
  ROS_ERROR_NAMED("mount", "MountConfigure: %s", ex.what());