ardrone_driver.cpp

Go to the documentation of this file.

#include <signal.h>
#include <string>
#include <algorithm>
#include <vector>

#include <ardrone_autonomy/ardrone_driver.h>
#include <ardrone_autonomy/teleop_twist.h>
#include <ardrone_autonomy/video.h>


// class ARDroneDriver

ARDroneDriver::ARDroneDriver()
  : private_nh("~"),
    image_transport(node_handle),
    // Ugly: This has been defined in the template file. Cleaner way to guarantee initilaztion?
    initialized_navdata_publishers(false),
    last_frame_id(-1),
    last_navdata_id(-1),
    is_inited(false),
    last_receive_time(0.0)
{
  cmd_vel_sub = node_handle.subscribe("cmd_vel", 1, &CmdVelCallback);
  takeoff_sub = node_handle.subscribe("ardrone/takeoff", 1, &TakeoffCallback);
  reset_sub = node_handle.subscribe("ardrone/reset", 1, &ResetCallback);
  land_sub = node_handle.subscribe("ardrone/land", 1, &LandCallback);
  image_pub = image_transport.advertiseCamera("ardrone/image_raw", 10);
  hori_pub = image_transport.advertiseCamera("ardrone/front/image_raw", 10);
  vert_pub = image_transport.advertiseCamera("ardrone/bottom/image_raw", 10);
  toggle_cam_srv = node_handle.advertiseService("ardrone/togglecam", ToggleCamCallback);
  set_cam_channel_srv = node_handle.advertiseService("ardrone/setcamchannel", SetCamChannelCallback);
  set_led_animation_srv = node_handle.advertiseService("ardrone/setledanimation", SetLedAnimationCallback);
  flat_trim_srv = node_handle.advertiseService("ardrone/flattrim", FlatTrimCallback);
  set_flight_anim_srv = node_handle.advertiseService("ardrone/setflightanimation", SetFlightAnimationCallback);
  set_record_srv = node_handle.advertiseService("ardrone/setrecord", SetRecordCallback);

  /* TF Frames */
  private_nh.param<std::string>("drone_frame_id", drone_frame_id, "ardrone_base");
  drone_frame_base = drone_frame_id + "_link";
  drone_frame_imu = drone_frame_id + "_imu";
  drone_frame_front_cam = drone_frame_id + "_frontcam";
  drone_frame_bottom_cam = drone_frame_id + "_bottomcam";

  if (private_nh.hasParam("root_frame"))
  {
    ROS_WARN("Changing `root_frame` has been deprecated since version 1.4. ");
  }

  // Fill constant parts of IMU Message
  // If no rosparam is set then the default value of 0.0 will be assigned to all covariance values
  for (int i = 0; i < 9; i++)
  {
    imu_msg.linear_acceleration_covariance[i] = 0.0;
    imu_msg.angular_velocity_covariance[i] = 0.0;
    imu_msg.orientation_covariance[i] = 0.0;
  }
  ReadCovParams("~cov/imu_la", imu_msg.linear_acceleration_covariance);
  ReadCovParams("~cov/imu_av", imu_msg.angular_velocity_covariance);
  ReadCovParams("~cov/imu_or", imu_msg.orientation_covariance);

  if (private_nh.hasParam("do_imu_caliberation"))
  {
    ROS_WARN("IMU Caliberation has been deprecated since 1.4.");
  }

  // Camera Info Manager
  cinfo_hori = new camera_info_manager::CameraInfoManager(ros::NodeHandle("ardrone/front"), "ardrone_front");
  cinfo_vert = new camera_info_manager::CameraInfoManager(ros::NodeHandle("ardrone/bottom"), "ardrone_bottom");

  // TF Stuff


  // Front Cam to Base
  // TODO(mani-monaj): Precise values for Drone1 & Drone2
  tf_base_front = tf::StampedTransform(
                    tf::Transform(
                      tf::createQuaternionFromRPY(-90.0 * _DEG2RAD, 0.0, -90.0 * _DEG2RAD),
                      tf::Vector3(0.21, 0.0, 0.0)),
                    ros::Time::now(), drone_frame_base, drone_frame_front_cam);

  // Bottom Cam to Base (Bad Assumption: No translation from IMU and Base)
  // TODO(mani-monaj): This should be different from Drone 1 & 2.
  tf_base_bottom = tf::StampedTransform(
                     tf::Transform(
                       tf::createQuaternionFromRPY(180.0 * _DEG2RAD, 0.0, 90.0 * _DEG2RAD),
                       tf::Vector3(0.0, -0.02, 0.0)),
                     ros::Time::now(), drone_frame_base, drone_frame_bottom_cam);

  // reset odometry
  odometry[0] = odometry[1] = 0;
}

ARDroneDriver::~ARDroneDriver()
{
  delete cinfo_hori;
  delete cinfo_vert;
}

void ARDroneDriver::run()
{
  ros::Rate loop_rate(looprate);
  const ros::Time startTime = ros::Time::now();
  static int freq_dev = 0;
  while (node_handle.ok())
  {
    if (!is_inited)  // Give the Drone 5s of free time to finish init phase
    {
      if (((ros::Time::now() - startTime).toSec()) > 5.0)
      {
        is_inited = true;

        // Send the configuration to the drone
        ConfigureDrone();

        vp_os_mutex_lock(&navdata_lock);
        ROS_INFO("Successfully connected to '%s' (AR-Drone %d.0 - Firmware: %s) - Battery(%%): %d",
                 ardrone_control_config.ardrone_name,
                 (IS_ARDRONE1) ? 1 : 2,
                 ardrone_control_config.num_version_soft,
                 shared_raw_navdata_ptr->navdata_demo.vbat_flying_percentage);
        ROS_INFO("Navdata Publish Settings:");
        ROS_INFO("    Legacy Navdata Mode: %s", enabled_legacy_navdata ? "On" : "Off");
        ROS_INFO("    ROS Loop Rate: %d Hz", looprate);
        ROS_INFO("    Realtime Navdata Publish: %s", realtime_navdata ? "On" : "Off");
        ROS_INFO("    Realtime Video Publish: %s", realtime_video ? "On" : "Off");
        ROS_INFO("    Drone Navdata Send Speed: %s",
                 ardrone_application_default_config.navdata_demo == 0 ?
                   "200Hz (navdata_demo=0)" : "15Hz (navdata_demo=1)");
        // TODO(mani-monaj): Enabled Navdata Demo
        vp_os_mutex_unlock(&navdata_lock);
        if (ardrone_control_config.num_version_soft[0] == '0')
        {
          ROS_WARN("The AR-Drone has a suspicious Firmware number. It usually means the network link is unreliable.");
        }
      }
    }
    else
    {
      if (!realtime_video)
      {
        vp_os_mutex_lock(&video_lock);
        copy_current_frame_id = current_frame_id;
        vp_os_mutex_unlock(&video_lock);
        if (copy_current_frame_id != last_frame_id)
        {
          last_frame_id = copy_current_frame_id;
          PublishVideo();
        }
      }

      if (!realtime_navdata)
      {
        vp_os_mutex_lock(&navdata_lock);
        copy_current_navdata_id = current_navdata_id;
        vp_os_mutex_unlock(&navdata_lock);
        if (copy_current_navdata_id != last_navdata_id)
        {
          vp_os_mutex_lock(&navdata_lock);
          last_navdata_id = copy_current_navdata_id;

          // Thread safe copy of interesting Navdata data
          // TODO(mani-monaj): This is a very expensive task, can we optimize here?
          // maybe ignoring the copy when it is not needed.
          const navdata_unpacked_t navdata_raw = *shared_raw_navdata_ptr;
          const ros::Time navdata_receive_time = shared_navdata_receive_time;
          vp_os_mutex_unlock(&navdata_lock);

          // This function is defined in the template NavdataMessageDefinitions.h template file
          PublishNavdataTypes(navdata_raw, navdata_receive_time);
          PublishNavdata(navdata_raw, navdata_receive_time);
          PublishOdometry(navdata_raw, navdata_receive_time);
        }
      }
      if (freq_dev == 0) PublishTF();

      // (looprate / 5)Hz  TF publish
      // TODO(mani-monaj): Make TF publish rate fixed
      freq_dev = (freq_dev + 1) % 5;
    }
    ros::spinOnce();
    loop_rate.sleep();
  }
  printf("ROS loop terminated ... \n");
}

void ARDroneDriver::ConfigureDrone()
{
  #include "ardrone_autonomy/snippet_configure_drone.h"
}

double ARDroneDriver::CalcAverage(const std::vector<double> &vec)
{
  double ret = 0.0;
  for (unsigned int i = 0; i < vec.size(); i++)
  {
    ret += vec[i];
  }
  return (ret / vec.size());
}

bool ARDroneDriver::ReadCovParams(const std::string &param_name, boost::array<double, 9> &cov_array)
{
  XmlRpc::XmlRpcValue cov_list;
  std::stringstream str_stream;
  if (ros::param::get(param_name, cov_list))
  {
    if (cov_list.getType() != XmlRpc::XmlRpcValue::TypeArray)
    {
      ROS_WARN("Covariance values for %s is not a list", param_name.c_str());
      return false;
    }

    if (cov_list.size() != 9)
    {
      ROS_WARN("Covariance list size for %s is not of size 9 (Size: %d)", param_name.c_str(), cov_list.size());
      return false;
    }
    str_stream << param_name << " set to [";
    for (int32_t i = 0; i < cov_list.size(); i++)
    {
      ROS_ASSERT(cov_list[i].getType() == XmlRpc::XmlRpcValue::TypeDouble);
      cov_array[i] = static_cast<double>(cov_list[i]);
      str_stream << cov_array[i] << ((i < 8) ? ", " : "");
    }
    str_stream << "]";
    ROS_INFO("%s", str_stream.str().c_str());
    return true;
  }
  else
  {
    ROS_INFO("No values found for `%s` in parameters, set all to zero.", param_name.c_str());
    return false;
  }
}

void ARDroneDriver::PublishVideo()
{
  if (
    (image_pub.getNumSubscribers() == 0) &&
    (hori_pub.getNumSubscribers() == 0) &&
    (vert_pub.getNumSubscribers() == 0)
  ) return;

  // Camera Info (NO PIP)

  sensor_msgs::CameraInfo cinfo_msg_hori = cinfo_hori->getCameraInfo();
  sensor_msgs::CameraInfo cinfo_msg_vert = cinfo_vert->getCameraInfo();

  cinfo_msg_hori.header.frame_id = drone_frame_front_cam;
  cinfo_msg_vert.header.frame_id = drone_frame_bottom_cam;

  if (IS_ARDRONE1)
  {
    /*
    * Information on buffer and image sizes.
    * Buffer is always in QVGA size, however for different Camera Modes
    * The picture and PIP sizes are different.
    *
    * image_raw and buffer are always 320x240. In order to preserve backward compatibilty image_raw remains
    * always as before. Two new set of topics are added for two new cameras : /ardrone/front/xxx and /ardrone/bottom/xxx
    *
    * In Camera State 0 front image relays the buffer  and image_raw and bottom image are not updated.
    *
    * In Camera State 1 bottom image is a 174x144 crop of the buffer. The front image is not updated
    *
    * In Camera State 2 bottom image is a PIP cut of size (87x72) from buffer.
    * The bottom image is a (320-87)x(240) cut of the buffer.
    *
    * In Camera State 3 front image is a PIP cut of size (58x42) from buffer.
    * The bottom image is a (174-58)x144 crop of the buffer.
    */
    sensor_msgs::Image image_msg;
    sensor_msgs::Image::_data_type::iterator _it;

    if ((cam_state == ZAP_CHANNEL_HORI) || (cam_state == ZAP_CHANNEL_LARGE_HORI_SMALL_VERT))
    {
      image_msg.header.frame_id = drone_frame_front_cam;
    }
    else if ((cam_state == ZAP_CHANNEL_VERT) || (cam_state == ZAP_CHANNEL_LARGE_VERT_SMALL_HORI))
    {
      image_msg.header.frame_id = drone_frame_bottom_cam;
    }
    else
    {
      ROS_WARN_ONCE("Something is wrong with camera channel config.");
    }

    image_msg.width = D1_STREAM_WIDTH;
    image_msg.height = D1_STREAM_HEIGHT;
    image_msg.encoding = "rgb8";
    image_msg.is_bigendian = false;
    image_msg.step = D1_STREAM_WIDTH * 3;
    image_msg.data.resize(D1_STREAM_WIDTH * D1_STREAM_HEIGHT * 3);

    if (!realtime_video) vp_os_mutex_lock(&video_lock);
    image_msg.header.stamp = shared_video_receive_time;
    std::copy(buffer, buffer + (D1_STREAM_WIDTH * D1_STREAM_HEIGHT * 3), image_msg.data.begin());
    if (!realtime_video) vp_os_mutex_unlock(&video_lock);

    cinfo_msg_hori.header.stamp = image_msg.header.stamp;
    cinfo_msg_vert.header.stamp = image_msg.header.stamp;

    if (cam_state == ZAP_CHANNEL_HORI)
    {
      /*
      * Horizontal camera is activated, only /ardrone/front/ is being updated
      */
      cinfo_msg_hori.width = D1_STREAM_WIDTH;
      cinfo_msg_hori.height = D1_STREAM_HEIGHT;

      image_pub.publish(image_msg, cinfo_msg_hori);
      hori_pub.publish(image_msg, cinfo_msg_hori);
    }
    else if (cam_state == ZAP_CHANNEL_VERT)
    {
      /*
      * Vertical camera is activated, only /ardrone/bottom/ is being updated
      */
      image_msg.width = D1_VERTSTREAM_WIDTH;
      image_msg.height = D1_VERTSTREAM_HEIGHT;
      image_msg.encoding = "rgb8";
      image_msg.is_bigendian = false;
      image_msg.step = D1_VERTSTREAM_WIDTH * 3;
      image_msg.data.clear();
      image_msg.data.resize(D1_VERTSTREAM_WIDTH * D1_VERTSTREAM_HEIGHT * 3);
      _it = image_msg.data.begin();
      if (!realtime_video) vp_os_mutex_lock(&video_lock);
      for (int row = 0; row < D1_VERTSTREAM_HEIGHT ; row++)
      {
        int _b = row * D1_STREAM_WIDTH * 3;
        int _e = _b + image_msg.step;
        _it = std::copy(buffer + _b, buffer + _e, _it);
      }
      if (!realtime_video) vp_os_mutex_unlock(&video_lock);

      cinfo_msg_vert.width = D1_VERTSTREAM_WIDTH;
      cinfo_msg_vert.height = D1_VERTSTREAM_HEIGHT;
      image_pub.publish(image_msg, cinfo_msg_vert);
      vert_pub.publish(image_msg, cinfo_msg_vert);
    }
    else if (cam_state == ZAP_CHANNEL_LARGE_HORI_SMALL_VERT)
    {
      /*
      * The Picture in Picture is activated with vertical camera inside the horizontal
      * camera. Both /ardrone/front and /ardrone/bottom is being updated
      */

      // Front (Cropping the first 88 columns)
      image_msg.width = D1_STREAM_WIDTH  - D1_MODE2_PIP_WIDTH;
      image_msg.height = D1_STREAM_HEIGHT;
      image_msg.encoding = "rgb8";
      image_msg.is_bigendian = false;
      image_msg.step = (D1_STREAM_WIDTH - D1_MODE2_PIP_WIDTH) * 3;
      image_msg.data.clear();
      image_msg.data.resize((D1_STREAM_WIDTH - D1_MODE2_PIP_WIDTH)*D1_STREAM_HEIGHT * 3);
      _it = image_msg.data.begin();
      if (!realtime_video) vp_os_mutex_lock(&video_lock);
      for (int row = 0; row < D1_STREAM_HEIGHT; row++)
      {
        int _b = (row * D1_STREAM_WIDTH * 3) + (D1_MODE2_PIP_WIDTH * 3);
        int _e = _b + image_msg.step;
        _it = std::copy(buffer + _b, buffer + _e, _it);
      }
      if (!realtime_video) vp_os_mutex_unlock(&video_lock);

      cinfo_msg_hori.width = D1_STREAM_WIDTH - D1_MODE2_PIP_WIDTH;
      cinfo_msg_hori.height = D1_STREAM_HEIGHT;
      hori_pub.publish(image_msg, cinfo_msg_hori);

      // Bottom
      image_msg.width = D1_MODE2_PIP_WIDTH;
      image_msg.height = D1_MODE2_PIP_HEIGHT;
      image_msg.encoding = "rgb8";
      image_msg.is_bigendian = false;
      image_msg.step = D1_MODE2_PIP_WIDTH * 3;
      image_msg.data.clear();
      image_msg.data.resize(D1_MODE2_PIP_WIDTH * D1_MODE2_PIP_HEIGHT * 3);
      _it = image_msg.data.begin();
      if (!realtime_video) vp_os_mutex_lock(&video_lock);
      for (int row = 0; row < D1_MODE2_PIP_HEIGHT; row++)
      {
        int _b = row * D1_STREAM_WIDTH * 3;
        int _e = _b + image_msg.step;
        _it = std::copy(buffer + _b, buffer + _e, _it);
      }
      if (!realtime_video) vp_os_mutex_unlock(&video_lock);

      cinfo_msg_vert.width = D1_MODE2_PIP_WIDTH;
      cinfo_msg_vert.height = D1_MODE2_PIP_HEIGHT;
      vert_pub.publish(image_msg, cinfo_msg_vert);
    }
    else if (cam_state == ZAP_CHANNEL_LARGE_VERT_SMALL_HORI)
    {
      /*
      * The Picture in Picture is activated with horizontal camera inside the vertical
      * camera. Both /ardrone/front and /ardrone/bottom is being updated
      */

      // Bottom  (Cropping the first 58 columns)
      image_msg.width = D1_VERTSTREAM_WIDTH   - D1_MODE3_PIP_WIDTH;
      image_msg.height = D1_VERTSTREAM_HEIGHT;
      image_msg.encoding = "rgb8";
      image_msg.is_bigendian = false;
      image_msg.step = (D1_VERTSTREAM_WIDTH - D1_MODE3_PIP_WIDTH) * 3;
      image_msg.data.clear();
      image_msg.data.resize((D1_VERTSTREAM_WIDTH - D1_MODE3_PIP_WIDTH)* D1_VERTSTREAM_HEIGHT * 3);
      _it = image_msg.data.begin();
      for (int row = 0; row < D1_VERTSTREAM_HEIGHT; row++)
      {
        int _b = (row * (D1_STREAM_WIDTH * 3)) + (D1_MODE3_PIP_WIDTH * 3);
        int _e = _b + image_msg.step;
        if (!realtime_video) vp_os_mutex_lock(&video_lock);
        _it = std::copy(buffer + _b, buffer + _e, _it);
        if (!realtime_video) vp_os_mutex_unlock(&video_lock);
      }

      cinfo_msg_vert.width = D1_VERTSTREAM_WIDTH - D1_MODE3_PIP_WIDTH;
      cinfo_msg_vert.height = D1_VERTSTREAM_HEIGHT;
      vert_pub.publish(image_msg, cinfo_msg_vert);

      // Front
      image_msg.width = D1_MODE3_PIP_WIDTH;
      image_msg.height = D1_MODE3_PIP_HEIGHT;
      image_msg.encoding = "rgb8";
      image_msg.is_bigendian = false;
      image_msg.step = D1_MODE3_PIP_WIDTH * 3;
      image_msg.data.clear();
      image_msg.data.resize(D1_MODE3_PIP_WIDTH * D1_MODE3_PIP_HEIGHT * 3);
      _it = image_msg.data.begin();
      if (!realtime_video) vp_os_mutex_lock(&video_lock);
      for (int row = 0; row < D1_MODE3_PIP_HEIGHT; row++)
      {
        int _b = row * D1_STREAM_WIDTH * 3;
        int _e = _b + image_msg.step;
        _it = std::copy(buffer + _b, buffer + _e, _it);
      }
      if (!realtime_video) vp_os_mutex_unlock(&video_lock);

      cinfo_msg_hori.width = D1_MODE3_PIP_WIDTH;
      cinfo_msg_hori.height = D1_MODE3_PIP_HEIGHT;
      hori_pub.publish(image_msg, cinfo_msg_hori);
    }
  }

  if (IS_ARDRONE2)
  {
    sensor_msgs::Image image_msg;
    sensor_msgs::Image::_data_type::iterator _it;

    if (cam_state == ZAP_CHANNEL_HORI)
    {
      image_msg.header.frame_id = drone_frame_front_cam;
    }
    else if (cam_state == ZAP_CHANNEL_VERT)
    {
      image_msg.header.frame_id = drone_frame_bottom_cam;
    }
    else
    {
      ROS_WARN_ONCE("Something is wrong with camera channel config.");
    }

    image_msg.width = D2_STREAM_WIDTH;
    image_msg.height = D2_STREAM_HEIGHT;
    image_msg.encoding = "rgb8";
    image_msg.is_bigendian = false;
    image_msg.step = D2_STREAM_WIDTH * 3;
    image_msg.data.resize(D2_STREAM_WIDTH * D2_STREAM_HEIGHT * 3);
    if (!realtime_video) vp_os_mutex_lock(&video_lock);
    image_msg.header.stamp = shared_video_receive_time;
    std::copy(buffer, buffer + (D2_STREAM_WIDTH * D2_STREAM_HEIGHT * 3), image_msg.data.begin());
    if (!realtime_video) vp_os_mutex_unlock(&video_lock);
    // We only put the width and height in here.

    cinfo_msg_hori.header.stamp = image_msg.header.stamp;
    cinfo_msg_vert.header.stamp = image_msg.header.stamp;

    if (cam_state == ZAP_CHANNEL_HORI)
    {
      /*
      * Horizontal camera is activated, only /ardrone/front/ is being updated
      */
      cinfo_msg_hori.width = D2_STREAM_WIDTH;
      cinfo_msg_hori.height = D2_STREAM_HEIGHT;
      image_pub.publish(image_msg, cinfo_msg_hori);  // /ardrone
      hori_pub.publish(image_msg, cinfo_msg_hori);
    }
    else if (cam_state == ZAP_CHANNEL_VERT)
    {
      /*
      * Vertical camera is activated, only /ardrone/bottom/ is being updated
      */
      cinfo_msg_vert.width = D2_STREAM_WIDTH;
      cinfo_msg_vert.height = D2_STREAM_HEIGHT;
      image_pub.publish(image_msg, cinfo_msg_vert);  // /ardrone
      vert_pub.publish(image_msg, cinfo_msg_vert);
    }
  }
}

void ARDroneDriver::PublishNavdata(const navdata_unpacked_t &navdata_raw, const ros::Time &navdata_receive_time)
{
  if (!enabled_legacy_navdata ||
      ((navdata_pub.getNumSubscribers() == 0) &&
       (imu_pub.getNumSubscribers() == 0) &&
       (mag_pub.getNumSubscribers() == 0)))
    return;  // why bother, no one is listening.

  legacynavdata_msg.header.stamp = navdata_receive_time;
  legacynavdata_msg.header.frame_id = drone_frame_base;
  legacynavdata_msg.batteryPercent = navdata_raw.navdata_demo.vbat_flying_percentage;
  legacynavdata_msg.state = (navdata_raw.navdata_demo.ctrl_state >> 16);

  // positive means counterclockwise rotation around axis
  legacynavdata_msg.rotX = navdata_raw.navdata_demo.phi / 1000.0;  // tilt left/right
  legacynavdata_msg.rotY = -navdata_raw.navdata_demo.theta / 1000.0;  // tilt forward/backward
  legacynavdata_msg.rotZ = -navdata_raw.navdata_demo.psi / 1000.0;  // orientation

  legacynavdata_msg.altd = navdata_raw.navdata_demo.altitude;  // cm
  legacynavdata_msg.vx = navdata_raw.navdata_demo.vx;  // mm/sec
  legacynavdata_msg.vy = -navdata_raw.navdata_demo.vy;  // mm/sec
  legacynavdata_msg.vz = -navdata_raw.navdata_demo.vz;  // mm/sec
  // msg.tm = navdata_raw.navdata_time.time;
  // First 21 bits (LSB) are usecs + 11 HSB are seconds
  legacynavdata_msg.tm = (navdata_raw.navdata_time.time & 0x001FFFFF) + (navdata_raw.navdata_time.time >> 21) * 1000000;
  legacynavdata_msg.ax = navdata_raw.navdata_phys_measures.phys_accs[ACC_X] / 1000.0;  // g
  legacynavdata_msg.ay = -navdata_raw.navdata_phys_measures.phys_accs[ACC_Y] / 1000.0;  // g
  legacynavdata_msg.az = -navdata_raw.navdata_phys_measures.phys_accs[ACC_Z] / 1000.0;  // g

  legacynavdata_msg.motor1 = navdata_raw.navdata_pwm.motor1;
  legacynavdata_msg.motor2 = navdata_raw.navdata_pwm.motor2;
  legacynavdata_msg.motor3 = navdata_raw.navdata_pwm.motor3;
  legacynavdata_msg.motor4 = navdata_raw.navdata_pwm.motor4;

  // New stuff

  if (IS_ARDRONE2)
  {
    legacynavdata_msg.magX = (int32_t)navdata_raw.navdata_magneto.mx;
    legacynavdata_msg.magY = (int32_t)navdata_raw.navdata_magneto.my;
    legacynavdata_msg.magZ = (int32_t)navdata_raw.navdata_magneto.mz;

    legacynavdata_msg.pressure = navdata_raw.navdata_pressure_raw.Pression_meas;  // typo in the SDK!
    legacynavdata_msg.temp = navdata_raw.navdata_pressure_raw.Temperature_meas;

    legacynavdata_msg.wind_speed = navdata_raw.navdata_wind_speed.wind_speed;
    legacynavdata_msg.wind_angle = navdata_raw.navdata_wind_speed.wind_angle;
    legacynavdata_msg.wind_comp_angle = navdata_raw.navdata_wind_speed.wind_compensation_phi;
  }
  else
  {
    legacynavdata_msg.magX = legacynavdata_msg.magY = legacynavdata_msg.magZ = 0;
    legacynavdata_msg.pressure = 0.0;
    legacynavdata_msg.temp = 0.0;
    legacynavdata_msg.wind_speed = 0.0;
    legacynavdata_msg.wind_angle = 0.0;
    legacynavdata_msg.wind_comp_angle = 0.0;
  }

  // Tag Detection, need to clear vectors first because it's a member variable now
  legacynavdata_msg.tags_type.clear();
  legacynavdata_msg.tags_xc.clear();
  legacynavdata_msg.tags_yc.clear();
  legacynavdata_msg.tags_width.clear();
  legacynavdata_msg.tags_height.clear();
  legacynavdata_msg.tags_orientation.clear();
  legacynavdata_msg.tags_distance.clear();

  legacynavdata_msg.tags_count = navdata_raw.navdata_vision_detect.nb_detected;
  for (int i = 0; i < navdata_raw.navdata_vision_detect.nb_detected; i++)
  {
    /*
     * The tags_type is in raw format. In order to extract the information
     * macros from ardrone_api.h is needed.
     *
     * #define DETECTION_MAKE_TYPE(source,tag) ( ((source)<<16) | (tag) )
     * #define DETECTION_EXTRACT_SOURCE(type)  ( ((type)>>16) & 0x0FF )
     * #define DETECTION_EXTRACT_TAG(type)     ( (type) & 0x0FF )
     *
     * Please also note that the xc, yc, width and height are in [0,1000] range
     * and must get converted back based on image resolution.
     */
    legacynavdata_msg.tags_type.push_back(navdata_raw.navdata_vision_detect.type[i]);
    legacynavdata_msg.tags_xc.push_back(navdata_raw.navdata_vision_detect.xc[i]);
    legacynavdata_msg.tags_yc.push_back(navdata_raw.navdata_vision_detect.yc[i]);
    legacynavdata_msg.tags_width.push_back(navdata_raw.navdata_vision_detect.width[i]);
    legacynavdata_msg.tags_height.push_back(navdata_raw.navdata_vision_detect.height[i]);
    legacynavdata_msg.tags_orientation.push_back(navdata_raw.navdata_vision_detect.orientation_angle[i]);
    legacynavdata_msg.tags_distance.push_back(navdata_raw.navdata_vision_detect.dist[i]);
  }

  /* IMU */
  imu_msg.header.frame_id = drone_frame_base;
  imu_msg.header.stamp = navdata_receive_time;

  // IMU - Linear Acc
  imu_msg.linear_acceleration.x = legacynavdata_msg.ax * 9.8;
  imu_msg.linear_acceleration.y = legacynavdata_msg.ay * 9.8;
  imu_msg.linear_acceleration.z = legacynavdata_msg.az * 9.8;

  // IMU - Rotation Matrix
  tf::Quaternion q;
  q.setRPY(legacynavdata_msg.rotX * _DEG2RAD, legacynavdata_msg.rotY * _DEG2RAD, legacynavdata_msg.rotZ * _DEG2RAD);
  tf::quaternionTFToMsg(q, imu_msg.orientation);

  // IMU - Gyro (Gyro is being sent in deg/sec)
  // TODO(mani-monaj): Should Gyro be added to Navdata?
  imu_msg.angular_velocity.x = navdata_raw.navdata_phys_measures.phys_gyros[GYRO_X] * DEG_TO_RAD;
  imu_msg.angular_velocity.y = -navdata_raw.navdata_phys_measures.phys_gyros[GYRO_Y] * DEG_TO_RAD;
  imu_msg.angular_velocity.z = -navdata_raw.navdata_phys_measures.phys_gyros[GYRO_Z] * DEG_TO_RAD;

  mag_msg.header.frame_id = drone_frame_base;
  mag_msg.header.stamp = navdata_receive_time;
  const float mag_normalizer = sqrt(legacynavdata_msg.magX * legacynavdata_msg.magX +
                                    legacynavdata_msg.magY * legacynavdata_msg.magY +
                                    legacynavdata_msg.magZ * legacynavdata_msg.magZ);

  // TODO(mani-monaj): Check if it is really needed that magnetometer message includes normalized value
  if (fabs(mag_normalizer) > 1e-9f)
  {
    mag_msg.vector.x = legacynavdata_msg.magX / mag_normalizer;
    mag_msg.vector.y = legacynavdata_msg.magY / mag_normalizer;
    mag_msg.vector.z = legacynavdata_msg.magZ / mag_normalizer;
    mag_pub.publish(mag_msg);
  }
  else
  {
    ROS_WARN_THROTTLE(30, "There is something wrong with the magnetometer readings (Magnitude is extremely small).");
  }

  navdata_pub.publish(legacynavdata_msg);
  imu_pub.publish(imu_msg);
}

// Load actual auto-generated code to publish full navdata
#define NAVDATA_STRUCTS_SOURCE
#include <ardrone_autonomy/NavdataMessageDefinitions.h>
#undef NAVDATA_STRUCTS_SOURCE

void ARDroneDriver::PublishTF()
{
  tf_base_front.stamp_ = ros::Time::now();
  tf_base_bottom.stamp_ = ros::Time::now();
  tf_odom.stamp_ = ros::Time::now();
  tf_broad.sendTransform(tf_base_front);
  tf_broad.sendTransform(tf_base_bottom);
  tf_broad.sendTransform(tf_odom);
}

void ARDroneDriver::PublishOdometry(const navdata_unpacked_t &navdata_raw, const ros::Time &navdata_receive_time)
{
  if (last_receive_time.isValid())
  {
    double delta_t = (navdata_receive_time - last_receive_time).toSec();
    odometry[0] += ((cos((navdata_raw.navdata_demo.psi / 180000.0) * M_PI) *
                     navdata_raw.navdata_demo.vx - sin((navdata_raw.navdata_demo.psi / 180000.0) * M_PI) *
                     -navdata_raw.navdata_demo.vy) * delta_t) / 1000.0;
    odometry[1] += ((sin((navdata_raw.navdata_demo.psi / 180000.0) * M_PI) *
                     navdata_raw.navdata_demo.vx + cos((navdata_raw.navdata_demo.psi / 180000.0) * M_PI) *
                     -navdata_raw.navdata_demo.vy) * delta_t) / 1000.0;
  }
  last_receive_time = navdata_receive_time;

  nav_msgs::Odometry odo_msg;
  odo_msg.header.stamp = navdata_receive_time;
  odo_msg.header.frame_id = "odom";
  odo_msg.child_frame_id = drone_frame_base;

  odo_msg.pose.pose.position.x = odometry[0];
  odo_msg.pose.pose.position.y = odometry[1];
  odo_msg.pose.pose.position.z = navdata_raw.navdata_demo.altitude / 1000;
  odo_msg.pose.pose.orientation = tf::createQuaternionMsgFromRollPitchYaw(
        navdata_raw.navdata_demo.phi / 180000.0 * M_PI,
        -navdata_raw.navdata_demo.theta / 180000.0 * M_PI,
        -navdata_raw.navdata_demo.psi / 180000.0 * M_PI);

  odo_msg.twist.twist.linear.x = navdata_raw.navdata_demo.vx / 1000.0;
  odo_msg.twist.twist.linear.y = -navdata_raw.navdata_demo.vy / 1000.0;
  odo_msg.twist.twist.linear.z = -navdata_raw.navdata_demo.vz / 1000.0;

  if (odo_pub.getNumSubscribers() > 0)
  {
    odo_pub.publish(odo_msg);
  }

  tf::Vector3 t;
  tf::pointMsgToTF(odo_msg.pose.pose.position, t);
  tf::Quaternion q;
  tf::quaternionMsgToTF(odo_msg.pose.pose.orientation, q);

  tf_odom.frame_id_ = "odom";
  tf_odom.child_frame_id_ = drone_frame_base;
  tf_odom.setOrigin(t);
  tf_odom.setRotation(q);
}

void ControlCHandler(int signal)
{
  ros::shutdown();
  should_exit = 1;
}

// custom_main

// extern "C" int custom_main(int argc, char** argv)
int main(int argc, char** argv)
{
  C_RESULT res = C_FAIL;
  char * drone_ip_address = NULL;

  // We need to implement our own Signal handler instead of ROS to shutdown
  // the SDK threads correctly.

  ros::init(argc, argv, "ardrone_driver", ros::init_options::NoSigintHandler);

  signal(SIGABRT, &ControlCHandler);
  signal(SIGTERM, &ControlCHandler);
  signal(SIGINT, &ControlCHandler);

  // Now to setup the drone and communication channels
  // We do this here because calling ardrone_tool_main uses an old
  // function initialization and is no longer recommended by parrot
  // I've based this section off the ControlEngine's initialization
  // routine (distributed with ARDrone SDK 2.0 Examples) as well as
  // the ardrone_tool_main function

  // Parse command line for
  // Backward compatibility with `-ip` command line argument
  argc--;
  argv++;
  while (argc && *argv[0] == '-')
  {
    if (!strcmp(*argv, "-ip") && (argc > 1))
    {
      drone_ip_address = *(argv + 1);
      printf("Using custom ip address %s\n", drone_ip_address);
      argc--;
      argv++;
    }
    argc--;
    argv++;
  }

  // Configure wifi
  vp_com_wifi_config_t *config = reinterpret_cast<vp_com_wifi_config_t*>(wifi_config());

  if (config)
  {
    vp_os_memset(&wifi_ardrone_ip[0], 0, ARDRONE_IPADDRESS_SIZE);

    // TODO(mani-monaj): Check if IP is valid
    if (drone_ip_address)
    {
      printf("===================+> %s\n", drone_ip_address);
      strncpy(&wifi_ardrone_ip[0], drone_ip_address, ARDRONE_IPADDRESS_SIZE - 1);
    }
    else
    {
      printf("===================+> %s\n", config->server);
      strncpy(&wifi_ardrone_ip[0], config->server, ARDRONE_IPADDRESS_SIZE - 1);
    }
  }

  while (-1 == getDroneVersion(".", wifi_ardrone_ip, &ardroneVersion))
  {
    printf("Getting AR.Drone version ...\n");
    vp_os_delay(250);
  }

  // Setup communication channels
  res = ardrone_tool_setup_com(NULL);
  if (FAILED(res))
  {
    PRINT("Wifi initialization failed. It means either:\n");
    PRINT("\t* you're not root (it's mandatory because you can set up wifi connection only as root)\n");
    PRINT("\t* wifi device is not present (on your pc or on your card)\n");
    PRINT("\t* you set the wrong name for wifi interface (for example rausb0 instead of wlan0) \n");
    PRINT("\t* ap is not up (reboot card or remove wifi usb dongle)\n");
    PRINT("\t* wifi device has no antenna\n");
  }
  else
  {
    // setup the application and user profiles for the driver
    const char* appname = reinterpret_cast<const char*>(DRIVER_APPNAME);
    const char* usrname = reinterpret_cast<const char*>(DRIVER_USERNAME);
    ardrone_gen_appid(appname, "2.0", app_id, app_name, APPLI_NAME_SIZE);
    ardrone_gen_usrid(usrname, usr_id, usr_name, USER_NAME_SIZE);

    // and finally initialize everything!
    // this will then call our sdk, which then starts the ::run() method of this file as an ardrone client thread

    res = ardrone_tool_init(wifi_ardrone_ip,
                            strlen(wifi_ardrone_ip),
                            NULL,
                            app_name,
                            usr_name,
                            NULL,
                            NULL,
                            MAX_FLIGHT_STORING_SIZE,
                            NULL);

    while (SUCCEED(res) && ardrone_tool_exit() == FALSE)
    {
      res = ardrone_tool_update();
    }
    res = ardrone_tool_shutdown();
  }
  return SUCCEED(res) ? 0 : -1;
}