aaa

  /* calculate virtual heading */
  double heading   = mrs_lib::AttitudeConverter(odom->pose.pose.orientation).getHeading();
  virtual_heading_ = mrs_lib::interpolateAngles(virtual_heading_, heading, _interpolate_coeff_);

  /* create reference stamped msg */
  mrs_msgs::ReferenceStampedSrv srv_reference_stamped_msg;

  /* fill in header */
  srv_reference_stamped_msg.request.header.stamp    = ros::Time::now();
  srv_reference_stamped_msg.request.header.frame_id = _uav_name_ + "/" + _frame_;

  /* fill in reference */
  srv_reference_stamped_msg.request.reference.position.x = odom->pose.pose.position.x + u * cos(virtual_heading_);
  srv_reference_stamped_msg.request.reference.position.y = odom->pose.pose.position.y + u * sin(virtual_heading_);
  srv_reference_stamped_msg.request.reference.position.z = _desired_height_;
  srv_reference_stamped_msg.request.reference.heading    = virtual_heading_ + w;

  virtual_heading_ += w;