gazebo_mavlink_interface.cpp

Go to the documentation of this file.

/*
 * Copyright 2015 Fadri Furrer, ASL, ETH Zurich, Switzerland
 * Copyright 2015 Michael Burri, ASL, ETH Zurich, Switzerland
 * Copyright 2015 Mina Kamel, ASL, ETH Zurich, Switzerland
 * Copyright 2015 Janosch Nikolic, ASL, ETH Zurich, Switzerland
 * Copyright 2015 Markus Achtelik, ASL, ETH Zurich, Switzerland
 * Copyright 2016 Geoffrey Hunter <gbmhunter@gmail.com>
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *     http://www.apache.org/licenses/LICENSE-2.0

 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 */

// MODULE
#include "rotors_gazebo_plugins/gazebo_mavlink_interface.h"

// USER
#include "rotors_gazebo_plugins/geo_mag_declination.h"

namespace gazebo {

// Set global reference point
// Zurich Irchel Park: 47.397742, 8.545594, 488m
// Seattle downtown (15 deg declination): 47.592182, -122.316031, 86m
// Moscow downtown: 55.753395, 37.625427, 155m

// Zurich Irchel Park
static const double kLatZurich_rad = 47.397742 * M_PI / 180;  // rad
static const double kLonZurich_rad = 8.545594 * M_PI / 180;  // rad
static const double kAltZurich_m = 488.0; // meters
// Seattle downtown (15 deg declination): 47.592182, -122.316031
// static const double lat_zurich = 47.592182 * M_PI / 180;  // rad
// static const double lon_zurich = -122.316031 * M_PI / 180;  // rad
// static const double alt_zurich = 86.0; // meters
static const float kEarthRadius_m = 6353000;  // m


GZ_REGISTER_MODEL_PLUGIN(GazeboMavlinkInterface);

GazeboMavlinkInterface::~GazeboMavlinkInterface() {
}

void GazeboMavlinkInterface::Load(physics::ModelPtr _model, sdf::ElementPtr _sdf) {

  if(kPrintOnPluginLoad) {
    gzdbg << __FUNCTION__ << "() called." << std::endl;
  }

  // Store the pointer to the model.
  model_ = _model;

  world_ = model_->GetWorld();

  namespace_.clear();
  if (_sdf->HasElement("robotNamespace")) {
    namespace_ = _sdf->GetElement("robotNamespace")->Get<std::string>();
    gzdbg << "namespace_ = \"" << namespace_ << "\"." << std::endl;
  } else {
    gzerr << "[gazebo_mavlink_interface] Please specify a robotNamespace.\n";
  }

  node_handle_ = transport::NodePtr(new transport::Node());
  node_handle_->Init();

  getSdfParam<std::string>(_sdf, "motorSpeedCommandPubTopic", motor_velocity_reference_pub_topic_,
                           motor_velocity_reference_pub_topic_);
  gzdbg << "motorSpeedCommandPubTopic = \"" << motor_velocity_reference_pub_topic_ << "\"." << std::endl;

  getSdfParam<std::string>(_sdf, "imuSubTopic", imu_sub_topic_, imu_sub_topic_);
  getSdfParam<std::string>(_sdf, "lidarSubTopic", lidar_sub_topic_, lidar_sub_topic_);
  getSdfParam<std::string>(_sdf, "opticalFlowSubTopic",
      opticalFlow_sub_topic_, opticalFlow_sub_topic_);

  // set input_reference_ from inputs.control
  input_reference_.resize(kNOutMax);
  joints_.resize(kNOutMax);
  pids_.resize(kNOutMax);
  for(int i = 0; i < kNOutMax; ++i)
  {
    pids_[i].Init(0, 0, 0, 0, 0, 0, 0);
    input_reference_[i] = 0;
  }

  if (_sdf->HasElement("control_channels")) {
    sdf::ElementPtr control_channels = _sdf->GetElement("control_channels");
    sdf::ElementPtr channel = control_channels->GetElement("channel");
    while (channel)
    {
      if (channel->HasElement("input_index"))
      {
        int index = channel->Get<int>("input_index");
        if (index < kNOutMax)
        {
          input_offset_[index] = channel->Get<double>("input_offset");
          input_scaling_[index] = channel->Get<double>("input_scaling");
          zero_position_disarmed_[index] = channel->Get<double>("zero_position_disarmed");
          zero_position_armed_[index] = channel->Get<double>("zero_position_armed");
          if (channel->HasElement("joint_control_type"))
          {
            joint_control_type_[index] = channel->Get<std::string>("joint_control_type");
          }
          else
          {
            gzwarn << "joint_control_type[" << index << "] not specified, using velocity.\n";
            joint_control_type_[index] = "velocity";
          }

          // start gz transport node handle
          if (joint_control_type_[index] == "position_gztopic")
          {
            // setup publisher handle to topic
            if (channel->HasElement("gztopic"))
              gztopic_[index] = "~/"+ model_->GetName() + channel->Get<std::string>("gztopic");
            else
              gztopic_[index] = "control_position_gztopic_" + std::to_string(index);
            #if (GAZEBO_MAJOR_VERSION >= 7 && GAZEBO_MINOR_VERSION >= 4) || GAZEBO_MAJOR_VERSION >= 8

              joint_control_pub_[index] = node_handle_->Advertise<gazebo::msgs::Any>(
                gztopic_[index]);
            #else
              joint_control_pub_[index] = node_handle_->Advertise<gazebo::msgs::GzString>(
                gztopic_[index]);
            #endif
          }

          if (channel->HasElement("joint_name"))
          {
            std::string joint_name = channel->Get<std::string>("joint_name");
            joints_[index] = model_->GetJoint(joint_name);
            if (joints_[index] == nullptr)
            {
              gzwarn << "joint [" << joint_name << "] not found for channel["
                     << index << "] no joint control for this channel.\n";
            }
            else
            {
              gzdbg << "joint [" << joint_name << "] found for channel["
                    << index << "] joint control active for this channel.\n";
            }
          }
          else
          {
            gzdbg << "<joint_name> not found for channel[" << index
                  << "] no joint control will be performed for this channel.\n";
          }

          // setup joint control pid to control joint
          if (channel->HasElement("joint_control_pid"))
          {
            sdf::ElementPtr pid = channel->GetElement("joint_control_pid");
            double p = 0;
            if (pid->HasElement("p"))
              p = pid->Get<double>("p");
            double i = 0;
            if (pid->HasElement("i"))
              i = pid->Get<double>("i");
            double d = 0;
            if (pid->HasElement("d"))
              d = pid->Get<double>("d");
            double iMax = 0;
            if (pid->HasElement("iMax"))
              iMax = pid->Get<double>("iMax");
            double iMin = 0;
            if (pid->HasElement("iMin"))
              iMin = pid->Get<double>("iMin");
            double cmdMax = 0;
            if (pid->HasElement("cmdMax"))
              cmdMax = pid->Get<double>("cmdMax");
            double cmdMin = 0;
            if (pid->HasElement("cmdMin"))
              cmdMin = pid->Get<double>("cmdMin");
            pids_[index].Init(p, i, d, iMax, iMin, cmdMax, cmdMin);
          }
        }
        else
        {
          gzerr << "input_index[" << index << "] out of range, not parsing.\n";
        }
      }
      else
      {
        gzerr << "no input_index, not parsing.\n";
        break;
      }
      channel = channel->GetNextElement("channel");
    }
  }

  if (_sdf->HasElement("left_elevon_joint")) {
    sdf::ElementPtr left_elevon =  _sdf->GetElement("left_elevon_joint");
    std::string left_elevon_joint_name = left_elevon->Get<std::string>();
    left_elevon_joint_ = model_->GetJoint(left_elevon_joint_name);
    int control_index;
    getSdfParam<int>(left_elevon, "input_index", control_index, -1);
    if (control_index >= 0) {
      joints_.at(control_index) = left_elevon_joint_;
    }
    // setup pid to control joint
    use_left_elevon_pid_ = false;
    if (left_elevon->HasElement("joint_control_pid")) {
      // setup joint control pid to control joint
      sdf::ElementPtr pid = _sdf->GetElement("joint_control_pid");
      double p = 0.1;
      if (pid->HasElement("p"))
        p = pid->Get<double>("p");
      double i = 0;
      if (pid->HasElement("i"))
        i = pid->Get<double>("i");
      double d = 0;
      if (pid->HasElement("d"))
        d = pid->Get<double>("d");
      double iMax = 0;
      if (pid->HasElement("iMax"))
        iMax = pid->Get<double>("iMax");
      double iMin = 0;
      if (pid->HasElement("iMin"))
        iMin = pid->Get<double>("iMin");
      double cmdMax = 3;
      if (pid->HasElement("cmdMax"))
        cmdMax = pid->Get<double>("cmdMax");
      double cmdMin = -3;
      if (pid->HasElement("cmdMin"))
        cmdMin = pid->Get<double>("cmdMin");
      left_elevon_pid_.Init(p, i, d, iMax, iMin, cmdMax, cmdMin);
      use_left_elevon_pid_ = true;
    }
  }

  if (_sdf->HasElement("left_aileron_joint")) {
    std::string left_elevon_joint_name = _sdf->GetElement("left_aileron_joint")->Get<std::string>();
    left_elevon_joint_ = model_->GetJoint(left_elevon_joint_name);
    int control_index;
    getSdfParam<int>(_sdf->GetElement("left_aileron_joint"), "input_index", control_index, -1);
    if (control_index >= 0) {
      joints_.at(control_index) = left_elevon_joint_;
    }
  }

  if (_sdf->HasElement("right_elevon_joint")) {
    sdf::ElementPtr right_elevon =  _sdf->GetElement("right_elevon_joint");
    std::string right_elevon_joint_name = right_elevon->Get<std::string>();
    right_elevon_joint_ = model_->GetJoint(right_elevon_joint_name);
    int control_index;
    getSdfParam<int>(right_elevon, "input_index", control_index, -1);
    if (control_index >= 0) {
      joints_.at(control_index) = right_elevon_joint_;
    }
    // setup pid to control joint
    use_right_elevon_pid_ = false;
    if (right_elevon->HasElement("joint_control_pid")) {
      // setup joint control pid to control joint
      sdf::ElementPtr pid = _sdf->GetElement("joint_control_pid");
      double p = 0.1;
      if (pid->HasElement("p"))
        p = pid->Get<double>("p");
      double i = 0;
      if (pid->HasElement("i"))
        i = pid->Get<double>("i");
      double d = 0;
      if (pid->HasElement("d"))
        d = pid->Get<double>("d");
      double iMax = 0;
      if (pid->HasElement("iMax"))
        iMax = pid->Get<double>("iMax");
      double iMin = 0;
      if (pid->HasElement("iMin"))
        iMin = pid->Get<double>("iMin");
      double cmdMax = 3;
      if (pid->HasElement("cmdMax"))
        cmdMax = pid->Get<double>("cmdMax");
      double cmdMin = -3;
      if (pid->HasElement("cmdMin"))
        cmdMin = pid->Get<double>("cmdMin");
      right_elevon_pid_.Init(p, i, d, iMax, iMin, cmdMax, cmdMin);
      use_right_elevon_pid_ = true;
    }
  }

  if (_sdf->HasElement("right_aileron_joint")) {
    std::string right_elevon_joint_name = _sdf->GetElement("right_aileron_joint")->Get<std::string>();
    right_elevon_joint_ = model_->GetJoint(right_elevon_joint_name);
    int control_index;
    getSdfParam<int>(_sdf->GetElement("right_aileron_joint"), "input_index", control_index, -1);
    if (control_index >= 0) {
      joints_.at(control_index) = right_elevon_joint_;
    }
  }

  if (_sdf->HasElement("elevator_joint")) {
    sdf::ElementPtr elevator =  _sdf->GetElement("elevator_joint");
    std::string elevator_joint_name = elevator->Get<std::string>();
    elevator_joint_ = model_->GetJoint(elevator_joint_name);
    int control_index;
    getSdfParam<int>(elevator, "input_index", control_index, -1);
    if (control_index >= 0) {
      joints_.at(control_index) = elevator_joint_;
    }
    // setup pid to control joint
    use_elevator_pid_ = false;
    if (elevator->HasElement("joint_control_pid")) {
      // setup joint control pid to control joint
      sdf::ElementPtr pid = _sdf->GetElement("joint_control_pid");
      double p = 0.1;
      if (pid->HasElement("p"))
        p = pid->Get<double>("p");
      double i = 0;
      if (pid->HasElement("i"))
        i = pid->Get<double>("i");
      double d = 0;
      if (pid->HasElement("d"))
        d = pid->Get<double>("d");
      double iMax = 0;
      if (pid->HasElement("iMax"))
        iMax = pid->Get<double>("iMax");
      double iMin = 0;
      if (pid->HasElement("iMin"))
        iMin = pid->Get<double>("iMin");
      double cmdMax = 3;
      if (pid->HasElement("cmdMax"))
        cmdMax = pid->Get<double>("cmdMax");
      double cmdMin = -3;
      if (pid->HasElement("cmdMin"))
        cmdMin = pid->Get<double>("cmdMin");
      elevator_pid_.Init(p, i, d, iMax, iMin, cmdMax, cmdMin);
      use_elevator_pid_ = true;
    }
  }

  if (_sdf->HasElement("propeller_joint")) {
    sdf::ElementPtr propeller =  _sdf->GetElement("propeller_joint");
    std::string propeller_joint_name = propeller->Get<std::string>();
    propeller_joint_ = model_->GetJoint(propeller_joint_name);
    int control_index;
    getSdfParam<int>(propeller, "input_index", control_index, -1);
    if (control_index >= 0) {
      joints_.at(control_index) = propeller_joint_;
    }
    use_propeller_pid_ = false;
    // setup joint control pid to control joint
    if (propeller->HasElement("joint_control_pid"))
    {
      sdf::ElementPtr pid = _sdf->GetElement("joint_control_pid");
      double p = 0.1;
      if (pid->HasElement("p"))
        p = pid->Get<double>("p");
      double i = 0;
      if (pid->HasElement("i"))
        i = pid->Get<double>("i");
      double d = 0;
      if (pid->HasElement("d"))
        d = pid->Get<double>("d");
      double iMax = 0;
      if (pid->HasElement("iMax"))
        iMax = pid->Get<double>("iMax");
      double iMin = 0;
      if (pid->HasElement("iMin"))
        iMin = pid->Get<double>("iMin");
      double cmdMax = 3;
      if (pid->HasElement("cmdMax"))
        cmdMax = pid->Get<double>("cmdMax");
      double cmdMin = -3;
      if (pid->HasElement("cmdMin"))
        cmdMin = pid->Get<double>("cmdMin");
      propeller_pid_.Init(p, i, d, iMax, iMin, cmdMax, cmdMin);
      use_propeller_pid_ = true;
    }
  }

  if (_sdf->HasElement("cgo3_mount_joint")) {
    std::string gimbal_yaw_joint_name = _sdf->GetElement("cgo3_mount_joint")->Get<std::string>();
    gimbal_yaw_joint_ = model_->GetJoint(gimbal_yaw_joint_name);
    int control_index;
    getSdfParam<int>(_sdf->GetElement("cgo3_mount_joint"), "input_index", control_index, -1);
    if (control_index >= 0) {
      joints_.at(control_index) = gimbal_yaw_joint_;
    }
  }

  if (_sdf->HasElement("cgo3_vertical_arm_joint")) {
    std::string gimbal_roll_joint_name = _sdf->GetElement("cgo3_vertical_arm_joint")->Get<std::string>();
    gimbal_roll_joint_ = model_->GetJoint(gimbal_roll_joint_name);
    int control_index;
    getSdfParam<int>(_sdf->GetElement("cgo3_vertical_arm_joint"), "input_index", control_index, -1);
    if (control_index >= 0) {
      joints_.at(control_index) = gimbal_roll_joint_;
    }
  }

  if (_sdf->HasElement("cgo3_horizontal_arm_joint")) {
    std::string gimbal_pitch_joint_name = _sdf->GetElement("cgo3_horizontal_arm_joint")->Get<std::string>();
    gimbal_pitch_joint_ = model_->GetJoint(gimbal_pitch_joint_name);
    int control_index;
    getSdfParam<int>(_sdf->GetElement("cgo3_horizontal_arm_joint"), "input_index", control_index, -1);
    if (control_index >= 0) {
      joints_.at(control_index) = gimbal_pitch_joint_;
    }
  }

  // Listen to the update event. This event is broadcast every
  // simulation iteration.
  updateConnection_ = event::Events::ConnectWorldUpdateBegin(
      boost::bind(&GazeboMavlinkInterface::OnUpdate, this, _1));

  //==============================================//
  //============ GAZEBO PUB/SUB SETUP ============//
  //==============================================//

  gzdbg << "Creating Gazebo subscriber on topic \"" << "~/" + namespace_ + "/" + imu_sub_topic_ << "\"." << std::endl;
  imu_sub_ = node_handle_->Subscribe("~/" + namespace_ + "/" + imu_sub_topic_, &GazeboMavlinkInterface::ImuCallback, this);

  gzdbg << "Creating Gazebo subscriber on topic \"" << "~/" + namespace_ + "/" + lidar_sub_topic_ << "\"." << std::endl;
  lidar_sub_ = node_handle_->Subscribe("~/" + namespace_ + "/" + lidar_sub_topic_, &GazeboMavlinkInterface::LidarCallback, this);

  gzdbg << "Creating Gazebo subscriber on topic \"" << "~/" + namespace_ + "/" + opticalFlow_sub_topic_ << "\"." << std::endl;
  opticalFlow_sub_ = node_handle_->Subscribe("~/" + namespace_ + "/" + opticalFlow_sub_topic_, &GazeboMavlinkInterface::OpticalFlowCallback, this);

  // Publish gazebo's motor_speed message
  gzdbg << "Creating Gazebo publisher on topic \"" << "~/" + namespace_ + "/" + motor_velocity_reference_pub_topic_ << "\"." << std::endl;
  motor_velocity_reference_pub_ = node_handle_->Advertise<gz_mav_msgs::CommandMotorSpeed>("~/" + namespace_ + "/" + motor_velocity_reference_pub_topic_, 1);

  // This topic is subscribed to by gazebo_geotagged_images_plugin.cpp
  gzdbg << "Creating Gazebo publisher on topic \"" << "~/gps_position" << "\"." << std::endl;
  gps_pub_ = node_handle_->Advertise<msgs::Vector3d>("~/gps_position");


  rotor_count_ = 5;
  last_time_ = world_->SimTime();
  last_gps_time_ = world_->SimTime();
  gps_update_interval_ = 0.2;  // in seconds for 5Hz

  gravity_W_ = world_->Gravity();

  // Magnetic field data for Zurich from WMM2015 (10^5xnanoTesla (N, E D) n-frame )
  // mag_n_ = {0.21523, 0.00771, -0.42741};
  // We set the world Y component to zero because we apply
  // the declination based on the global position,
  // and so we need to start without any offsets.
  // The real value for Zurich would be 0.00771
  // frame d is the magnetic north frame
  mag_d_.X() = 0.21523;
  mag_d_.Y() = 0;
  mag_d_.Z() = -0.42741;

  //Create socket
  // udp socket data
  mavlink_addr_ = htonl(INADDR_ANY);
  if (_sdf->HasElement("mavlink_addr")) {
    std::string mavlink_addr = _sdf->GetElement("mavlink_addr")->Get<std::string>();
    if (mavlink_addr != "INADDR_ANY") {
      mavlink_addr_ = inet_addr(mavlink_addr.c_str());
      if (mavlink_addr_ == INADDR_NONE) {
        fprintf(stderr, "invalid mavlink_addr \"%s\"\n", mavlink_addr.c_str());
        return;
      }
    }
  }
  if (_sdf->HasElement("mavlink_udp_port")) {
    mavlink_udp_port_ = _sdf->GetElement("mavlink_udp_port")->Get<int>();
  }

  // try to setup udp socket for communcation with simulator
  if ((fd_ = socket(AF_INET, SOCK_DGRAM, 0)) < 0) {
    printf("create socket failed\n");
    return;
  }

  memset((char *)&myaddr_, 0, sizeof(myaddr_));
  myaddr_.sin_family = AF_INET;
  myaddr_.sin_addr.s_addr = htonl(INADDR_ANY);
  // Let the OS pick the port
  myaddr_.sin_port = htons(0);

  if (bind(fd_, (struct sockaddr *)&myaddr_, sizeof(myaddr_)) < 0) {
    printf("bind failed\n");
    return;
  }

  srcaddr_.sin_family = AF_INET;
  srcaddr_.sin_addr.s_addr = mavlink_addr_;
  srcaddr_.sin_port = htons(mavlink_udp_port_);
  addrlen_ = sizeof(srcaddr_);

  fds_[0].fd = fd_;
  fds_[0].events = POLLIN;


}


void GazeboMavlinkInterface::OnUpdate(const common::UpdateInfo& /*_info*/) {

  common::Time current_time = world_->SimTime();
  double dt = (current_time - last_time_).Double();

  pollForMAVLinkMessages(dt, 1000);

  handle_control(dt);

  if(received_first_reference_) {

    gz_mav_msgs::CommandMotorSpeed turning_velocities_msg;

    for (int i = 0; i < input_reference_.size(); i++){
      if (last_actuator_time_ == 0 || (current_time - last_actuator_time_).Double() > 0.2) {
        turning_velocities_msg.add_motor_speed(0);
      } else {
        turning_velocities_msg.add_motor_speed(input_reference_[i]);
      }
    }

    // TODO Add timestamp and Header
    // turning_velocities_msg->header.stamp.sec = current_time.sec;
    // turning_velocities_msg->header.stamp.nsec = current_time.nsec;

    // gzerr << turning_velocities_msg.motor_speed(0) << "\n";
    motor_velocity_reference_pub_->Publish(turning_velocities_msg);
  }

  last_time_ = current_time;

  //send gps
  ignition::math::Pose3d T_W_I = model_->WorldPose(); //TODO(burrimi): Check tf.
  ignition::math::Vector3d pos_W_I = T_W_I.Pos();  // Use the models' world position for GPS and pressure alt.

  ignition::math::Vector3d velocity_current_W = model_->WorldLinearVel();  // Use the models' world position for GPS velocity.

  ignition::math::Vector3d velocity_current_W_xy = velocity_current_W;
  velocity_current_W_xy.Z() = 0;

  // TODO: Remove GPS message from IMU plugin. Added gazebo GPS plugin. This is temp here.
  // reproject local position to gps coordinates
  double x_rad = pos_W_I.Y() / kEarthRadius_m; // north
  double y_rad = pos_W_I.X() / kEarthRadius_m; // east
  double c = sqrt(x_rad * x_rad + y_rad * y_rad);
  double sin_c = sin(c);
  double cos_c = cos(c);
  if (c != 0.0) {
    lat_rad_ = asin(cos_c * sin(kLatZurich_rad) + (x_rad * sin_c * cos(kLatZurich_rad)) / c);
    lon_rad_ = (kLonZurich_rad + atan2(y_rad * sin_c, c * cos(kLatZurich_rad) * cos_c - x_rad * sin(kLatZurich_rad) * sin_c));
  } else {
   lat_rad_ = kLatZurich_rad;
    lon_rad_ = kLonZurich_rad;
  }

  if (current_time.Double() - last_gps_time_.Double() > gps_update_interval_) {  // 5Hz
    // Raw UDP mavlink
    mavlink_hil_gps_t hil_gps_msg;
    hil_gps_msg.time_usec = current_time.nsec/1000;
    hil_gps_msg.fix_type = 3;
    hil_gps_msg.lat = lat_rad_ * 180 / M_PI * 1e7;
    hil_gps_msg.lon = lon_rad_ * 180 / M_PI * 1e7;
    hil_gps_msg.alt = (pos_W_I.Z() + kAltZurich_m) * 1000;
    hil_gps_msg.eph = 100;
    hil_gps_msg.epv = 100;
    hil_gps_msg.vel = velocity_current_W_xy.Length() * 100;
    hil_gps_msg.vn = velocity_current_W.Y() * 100;
    hil_gps_msg.ve = velocity_current_W.X() * 100;
    hil_gps_msg.vd = -velocity_current_W.Z() * 100;
    hil_gps_msg.cog = atan2(hil_gps_msg.ve, hil_gps_msg.vn) * 180.0/3.1416 * 100.0;
    hil_gps_msg.satellites_visible = 10;

    /*static int gps_debug_msg_count = 0;
    if(gps_debug_msg_count >= 0) {
      gzmsg << "{ "
      "time_usec: " << hil_gps_msg.time_usec <<
      ", fix_type: " << hil_gps_msg.fix_type <<
      ", lat: " << hil_gps_msg.lat <<
      ", lon: " << hil_gps_msg.lon <<
      ", alt: " << hil_gps_msg.alt <<
      ", eph: " << hil_gps_msg.eph <<
      ", epv: " << hil_gps_msg.epv <<
      ", vel: " << hil_gps_msg.vel <<
      ", vv: " << hil_gps_msg.vn <<
      ", ve: " << hil_gps_msg.ve <<
      ", vd: " << hil_gps_msg.vd <<
      ", cog: " << hil_gps_msg.cog <<
      ", satellites_visible: " << static_cast<int>(hil_gps_msg.satellites_visible) <<
      " }" << std::endl;
      gps_debug_msg_count = 0;
    }
    gps_debug_msg_count++;*/

    send_mavlink_message(MAVLINK_MSG_ID_HIL_GPS, &hil_gps_msg, 200);

    // Also publish GPS info on Gazebo topic
    msgs::Vector3d gps_msg;
    gps_msg.set_x(lat_rad_ * 180. / M_PI);
    gps_msg.set_y(lon_rad_ * 180. / M_PI);
    gps_msg.set_z(hil_gps_msg.alt / 1000.f);
    gps_pub_->Publish(gps_msg);

    last_gps_time_ = current_time;
  }
}

void GazeboMavlinkInterface::send_mavlink_message(const uint8_t msgid, const void *msg, uint8_t component_ID) {

  component_ID = 0;
  uint8_t payload_len = mavlink_message_lengths[msgid];
  unsigned packet_len = payload_len + MAVLINK_NUM_NON_PAYLOAD_BYTES;

  uint8_t buf[MAVLINK_MAX_PACKET_LEN];

  // Header
  buf[0] = MAVLINK_STX;
  buf[1] = payload_len;
  // no idea which numbers should be here
  buf[2] = 100;
  buf[3] = 0;
  buf[4] = component_ID;
  buf[5] = msgid;

  // Payload
  memcpy(&buf[MAVLINK_NUM_HEADER_BYTES],msg, payload_len);

  // Checksym
  uint16_t checksum;
  crc_init(&checksum);
  crc_accumulate_buffer(&checksum, (const char *) &buf[1], MAVLINK_CORE_HEADER_LEN + payload_len);
  crc_accumulate(mavlink_message_crcs[msgid], &checksum);

  buf[MAVLINK_NUM_HEADER_BYTES + payload_len] = (uint8_t)(checksum & 0xFF);
  buf[MAVLINK_NUM_HEADER_BYTES + payload_len + 1] = (uint8_t)(checksum >> 8);

  ssize_t len;

  len = sendto(fd_, buf, packet_len, 0, (struct sockaddr *)&srcaddr_, sizeof(srcaddr_));

  if (len <= 0) {
    printf("Failed sending mavlink message\n");
  }
}

void GazeboMavlinkInterface::ImuCallback(ImuPtr& imu_message) {

  //gzdbg << __FUNCTION__ << "() called." << std::endl;

  /*static int imu_debug_msg_count = 0;
  if(imu_debug_msg_count >= 100) {
    gzmsg << "{ "
    "w: " << imu_message->orientation().w() <<
    ", x: " << imu_message->orientation().x() <<
    ", y: " << imu_message->orientation().y() <<
    ", z: " << imu_message->orientation().z() <<
    " }" << std::endl;
    imu_debug_msg_count = 0;
  }
  imu_debug_msg_count++;*/

  // frames
  // g - gazebo (ENU), east, north, up
  // r - rotors imu frame (FLU), forward, left, up
  // b - px4 (FRD) forward, right down
  // n - px4 (NED) north, east, down
  ignition::math::Quaterniond q_gr = ignition::math::Quaterniond(
    imu_message->orientation().w(),
    imu_message->orientation().x(),
    imu_message->orientation().y(),
    imu_message->orientation().z());


  // q_br
  /*
  tf.euler2quat(*tf.mat2euler([
  #        F  L  U
          [1, 0, 0],  # F
          [0, -1, 0], # R
          [0, 0, -1]  # D
      ]
  )).round(5)
  */
  ignition::math::Quaterniond q_br(0, 1, 0, 0);


  // q_ng
  /*
  tf.euler2quat(*tf.mat2euler([
  #        N  E  D
          [0, 1, 0],  # E
          [1, 0, 0],  # N
          [0, 0, -1]  # U
      ]
  )).round(5)
  */
  ignition::math::Quaterniond q_ng(0, 0.70711, 0.70711, 0);

  ignition::math::Quaterniond q_gb = q_gr*q_br.Inverse();
  ignition::math::Quaterniond q_nb = q_ng*q_gb;

  ignition::math::Vector3d pos_g = model_->WorldPose().Pos();
  ignition::math::Vector3d pos_n = q_ng.RotateVector(pos_g);

  //gzerr << "got imu: " << C_W_I << "\n";
  //gzerr << "got pose: " << T_W_I.rot << "\n";
  float declination = get_mag_declination(lat_rad_, lon_rad_);

  ignition::math::Quaterniond q_dn(0.0, 0.0, declination);
  ignition::math::Vector3d mag_n = q_dn.RotateVectorReverse(mag_d_);

  ignition::math::Vector3d vel_b = q_br.RotateVector(model_->RelativeLinearVel());
  ignition::math::Vector3d vel_n = q_ng.RotateVector(model_->WorldLinearVel());
  ignition::math::Vector3d omega_nb_b = q_br.RotateVector(model_->RelativeAngularVel());

  standard_normal_distribution_ = std::normal_distribution<float>(0, 0.01f);
  ignition::math::Vector3d mag_noise_b(
    standard_normal_distribution_(random_generator_),
    standard_normal_distribution_(random_generator_),
    standard_normal_distribution_(random_generator_));

  ignition::math::Vector3d accel_b = q_br.RotateVector(ignition::math::Vector3d (
    imu_message->linear_acceleration().x(),
    imu_message->linear_acceleration().y(),
    imu_message->linear_acceleration().z()));
  ignition::math::Vector3d gyro_b = q_br.RotateVector(ignition::math::Vector3d (
    imu_message->angular_velocity().x(),
    imu_message->angular_velocity().y(),
    imu_message->angular_velocity().z()));
  ignition::math::Vector3d mag_b = q_nb.RotateVectorReverse(mag_n) + mag_noise_b;

  mavlink_hil_sensor_t sensor_msg;
  sensor_msg.time_usec = world_->SimTime().nsec/1000;
  sensor_msg.xacc = accel_b.X();
  sensor_msg.yacc = accel_b.Y();
  sensor_msg.zacc = accel_b.Z();
  sensor_msg.xgyro = gyro_b.X();
  sensor_msg.ygyro = gyro_b.Y();
  sensor_msg.zgyro = gyro_b.Z();
  sensor_msg.xmag = mag_b.X();
  sensor_msg.ymag = mag_b.Y();
  sensor_msg.zmag = mag_b.Z();
  sensor_msg.abs_pressure = 0.0;
  float rho = 1.2754f; // density of air, TODO why is this not 1.225 as given by std. atmos.
  sensor_msg.diff_pressure = 0.5f*rho*vel_b.X()*vel_b.X() / 100;

  float p1, p2;

  // need to add noise to pressure alt
  do {
      p1 = rand() * (1.0 / RAND_MAX);
      p2 = rand() * (1.0 / RAND_MAX);
  } while (p1 <= __FLT_EPSILON__);

  float n = sqrtf(-2.0 * logf(p1)) * cosf(2.0f * M_PI * p2);
  float alt_n = -pos_n.Z() + n * sqrtf(0.006f);

  sensor_msg.pressure_alt = (std::isfinite(alt_n)) ? alt_n : -pos_n.Z();
  sensor_msg.temperature = 0.0;
  sensor_msg.fields_updated = 4095;

  //gyro needed for optical flow message
  optflow_xgyro_ = gyro_b.X();
  optflow_ygyro_ = gyro_b.Y();
  optflow_zgyro_ = gyro_b.Z();

  /*static int imu_msg_count = 0;
  if(imu_msg_count >= 100) {
    gzmsg << "{ "
    "time_usec: " << sensor_msg.time_usec <<
    ", xacc: " << sensor_msg.xacc <<
    ", yacc: " << sensor_msg.yacc <<
    ", zacc: " << sensor_msg.zacc <<
    ", xgyro: " << sensor_msg.xgyro <<
    ", ygyro: " << sensor_msg.ygyro <<
    ", zgyro: " << sensor_msg.zgyro <<
    ", xmag: " << sensor_msg.xmag <<
    ", ymag: " << sensor_msg.ymag <<
    ", zmag: " << sensor_msg.zmag <<
    ", abs_pressure: " << sensor_msg.abs_pressure <<
    ", diff_pressure: " << sensor_msg.diff_pressure <<
    ", pressure_alt: " << sensor_msg.pressure_alt <<
    ", temperature: " << sensor_msg.temperature <<
    ", fields_updated: " << sensor_msg.fields_updated <<
    " }" << std::endl;
    imu_msg_count = 0;
  }
  imu_msg_count++;*/

  send_mavlink_message(MAVLINK_MSG_ID_HIL_SENSOR, &sensor_msg, 200);

  // ground truth
  ignition::math::Vector3d accel_true_b = q_br.RotateVector(model_->RelativeLinearAccel());

  // send ground truth
  mavlink_hil_state_quaternion_t hil_state_quat;
  hil_state_quat.time_usec = world_->SimTime().nsec/1000;
  hil_state_quat.attitude_quaternion[0] = q_nb.W();
  hil_state_quat.attitude_quaternion[1] = q_nb.X();
  hil_state_quat.attitude_quaternion[2] = q_nb.Y();
  hil_state_quat.attitude_quaternion[3] = q_nb.Z();

  hil_state_quat.rollspeed = omega_nb_b.X();
  hil_state_quat.pitchspeed = omega_nb_b.Y();
  hil_state_quat.yawspeed = omega_nb_b.Z();

  hil_state_quat.lat = lat_rad_ * 180 / M_PI * 1e7;
  hil_state_quat.lon = lon_rad_ * 180 / M_PI * 1e7;
  hil_state_quat.alt = (-pos_n.Z() + kAltZurich_m) * 1000;

  hil_state_quat.vx = vel_n.X() * 100;
  hil_state_quat.vy = vel_n.Y() * 100;
  hil_state_quat.vz = vel_n.Z() * 100;

  // assumed indicated airspeed due to flow aligned with pitot (body x)
  hil_state_quat.ind_airspeed = vel_b.X();
  hil_state_quat.true_airspeed = model_->WorldLinearVel().Length() * 100; //no wind simulated

  hil_state_quat.xacc = accel_true_b.X() * 1000;
  hil_state_quat.yacc = accel_true_b.Y() * 1000;
  hil_state_quat.zacc = accel_true_b.Z() * 1000;

  /*static int quat_msg_count = 0;
  if(quat_msg_count >= 100) {
    gzmsg << "{ "
    "time_usec: " << hil_state_quat.time_usec <<
    ", attitude_quaternion[0]: " << hil_state_quat.attitude_quaternion[0] <<
    ", attitude_quaternion[1]: " << hil_state_quat.attitude_quaternion[1] <<
    ", attitude_quaternion[2]: " << hil_state_quat.attitude_quaternion[2] <<
    ", attitude_quaternion[3]: " << hil_state_quat.attitude_quaternion[3] <<
    ", rollspeed: " << hil_state_quat.rollspeed <<
    ", pitchspeed: " << hil_state_quat.pitchspeed <<
    ", yawspeed: " << hil_state_quat.yawspeed <<
    ", lat: " << hil_state_quat.lat <<
    ", lon: " << hil_state_quat.lon <<
    ", alt: " << hil_state_quat.alt <<
    ", vx: " << hil_state_quat.vx <<
    ", vy: " << hil_state_quat.vy <<
    ", vz: " << hil_state_quat.vz <<
    ", ind_airspeed: " << hil_state_quat.ind_airspeed <<
    ", true_airspeed: " << hil_state_quat.true_airspeed <<
    ", xacc: " << hil_state_quat.xacc <<
    ", yacc: " << hil_state_quat.yacc <<
    ", zacc: " << hil_state_quat.zacc <<
    " }" << std::endl;
    quat_msg_count = 0;
  }
  quat_msg_count++;*/

  send_mavlink_message(MAVLINK_MSG_ID_HIL_STATE_QUATERNION, &hil_state_quat, 200);
}

void GazeboMavlinkInterface::LidarCallback(LidarPtr& lidar_message) {

  gzdbg << __FUNCTION__ << "() called." << std::endl;

  mavlink_distance_sensor_t sensor_msg;
  sensor_msg.time_boot_ms = lidar_message->time_msec();
  sensor_msg.min_distance = lidar_message->min_distance() * 100.0;
  sensor_msg.max_distance = lidar_message->max_distance() * 100.0;
  sensor_msg.current_distance = lidar_message->current_distance() * 100.0;
  sensor_msg.type = 0;
  sensor_msg.id = 0;
  // to to roll 180 (downward facing for agl measurement)
  sensor_msg.orientation = 8;
  sensor_msg.covariance = 0;

  //distance needed for optical flow message
  optflow_distance_ = lidar_message->current_distance(); //[m]

  send_mavlink_message(MAVLINK_MSG_ID_DISTANCE_SENSOR, &sensor_msg, 200);

}

void GazeboMavlinkInterface::OpticalFlowCallback(OpticalFlowPtr& opticalFlow_message) {

  gzdbg << __FUNCTION__ << "() called." << std::endl;

  mavlink_hil_optical_flow_t sensor_msg;
  sensor_msg.time_usec = opticalFlow_message->time_usec();
  sensor_msg.sensor_id = opticalFlow_message->sensor_id();
  sensor_msg.integration_time_us = opticalFlow_message->integration_time_us();
  sensor_msg.integrated_x = opticalFlow_message->integrated_x();
  sensor_msg.integrated_y = opticalFlow_message->integrated_y();
  sensor_msg.integrated_xgyro = -optflow_ygyro_ * opticalFlow_message->integration_time_us() / 1000000.0; //xy switched
  sensor_msg.integrated_ygyro = optflow_xgyro_ * opticalFlow_message->integration_time_us() / 1000000.0; //xy switched
  sensor_msg.integrated_zgyro = -optflow_zgyro_ * opticalFlow_message->integration_time_us() / 1000000.0; //change direction
  sensor_msg.temperature = opticalFlow_message->temperature();
  sensor_msg.quality = opticalFlow_message->quality();
  sensor_msg.time_delta_distance_us = opticalFlow_message->time_delta_distance_us();
  sensor_msg.distance = optflow_distance_;

  send_mavlink_message(MAVLINK_MSG_ID_HIL_OPTICAL_FLOW, &sensor_msg, 200);
}

/*ssize_t GazeboMavlinkInterface::receive(void *_buf, const size_t _size, uint32_t _timeoutMs)
{
  fd_set fds;
  struct timeval tv;

  FD_ZERO(&fds);
  FD_SET(this->handle, &fds);

  tv.tv_sec = _timeoutMs / 1000;
  tv.tv_usec = (_timeoutMs % 1000) * 1000UL;

  if (select(this->handle+1, &fds, NULL, NULL, &tv) != 1)
  {
      return -1;
  }

  return recv(this->handle, _buf, _size, 0);
}*/

void GazeboMavlinkInterface::pollForMAVLinkMessages(double _dt, uint32_t _timeoutMs)
{
  // convert timeout in ms to timeval
  struct timeval tv;
  tv.tv_sec = _timeoutMs / 1000;
  tv.tv_usec = (_timeoutMs % 1000) * 1000UL;

  // poll
  ::poll(&fds_[0], (sizeof(fds_[0])/sizeof(fds_[0])), 0);

  if (fds_[0].revents & POLLIN) {
    int len = recvfrom(fd_, buf_, sizeof(buf_), 0, (struct sockaddr *)&srcaddr_, &addrlen_);
    if (len > 0) {
      mavlink_message_t msg;
      mavlink_status_t status;
      for (unsigned i = 0; i < len; ++i)
      {
        if (mavlink_parse_char(MAVLINK_COMM_0, buf_[i], &msg, &status))
        {
          // have a message, handle it
          handle_message(&msg);
        }
      }
    }
  }
}

void GazeboMavlinkInterface::handle_message(mavlink_message_t *msg)
{

  //gzdbg << __FUNCTION__ << "() called, msg->msgid = " << msg->msgid << "." << std::endl;

  switch(msg->msgid) {
  case MAVLINK_MSG_ID_HIL_ACTUATOR_CONTROLS:
    mavlink_hil_actuator_controls_t controls;
    mavlink_msg_hil_actuator_controls_decode(msg, &controls);
    bool armed = false;

    if ((controls.mode & MAV_MODE_FLAG_SAFETY_ARMED) > 0) {
      armed = true;
    }

    last_actuator_time_ = world_->SimTime();

    for (unsigned i = 0; i < kNOutMax; i++) {
      input_index_[i] = i;
    }

    // set rotor speeds, controller targets
    input_reference_.resize(kNOutMax);
    for (int i = 0; i < input_reference_.size(); i++) {
      if (armed) {
        input_reference_[i] = (controls.controls[input_index_[i]] + input_offset_[i])
          * input_scaling_[i] + zero_position_armed_[i];
        // if (joints_[i])
        //   gzerr << i << " : " << input_index_[i] << " : " << controls.controls[input_index_[i]] << " : " << input_reference_[i] << "\n";
      } else {
        input_reference_[i] = zero_position_disarmed_[i];
      }
    }

    received_first_reference_ = true;
    break;
  }
}

void GazeboMavlinkInterface::handle_control(double _dt)
{
    // set joint positions
    for (int i = 0; i < input_reference_.size(); i++) {
      if (joints_[i]) {
        double target = input_reference_[i];
        if (joint_control_type_[i] == "velocity")
        {
          double current = joints_[i]->GetVelocity(0);
          double err = current - target;
          double force = pids_[i].Update(err, _dt);
          joints_[i]->SetForce(0, force);
          // gzerr << "chan[" << i << "] curr[" << current
          //       << "] cmd[" << target << "] f[" << force
          //       << "] scale[" << input_scaling_[i] << "]\n";
        }
        else if (joint_control_type_[i] == "position")
        {
          double current = joints_[i]->Position(0);
          double err = current - target;
          double force = pids_[i].Update(err, _dt);
          joints_[i]->SetForce(0, force);
          // gzerr << "chan[" << i << "] curr[" << current
          //       << "] cmd[" << target << "] f[" << force
          //       << "] scale[" << input_scaling_[i] << "]\n";
        }
        else if (joint_control_type_[i] == "position_gztopic")
        {
          #if (GAZEBO_MAJOR_VERSION >= 7 && GAZEBO_MINOR_VERSION >= 4) || GAZEBO_MAJOR_VERSION >= 8

            gazebo::msgs::Any m;
            m.set_type(gazebo::msgs::Any_ValueType_DOUBLE);
            m.set_double_value(target);
          #else
            std::stringstream ss;
            gazebo::msgs::GzString m;
            ss << target;
            m.set_data(ss.str());
          #endif

          joint_control_pub_[i]->Publish(m);
        }
        else if (joint_control_type_[i] == "position_kinematic")
        {
            joints_[i]->SetPosition(0, input_reference_[i]);
        }
        else
        {
          gzerr << "joint_control_type[" << joint_control_type_[i] << "] undefined.\n";
        }
      }
    }
}

}