/**
* @brief Send vision estimate transform to FCU position controller
*/
void send_vision_estimate(const ros::Time &stamp, const Eigen::Affine3d &tr)
{
/**
* @warning Issue #60.
* This now affects pose callbacks too.
*/
if (last_transform_stamp == stamp) {
ROS_DEBUG_THROTTLE_NAMED(10, "vision_pose", "Vision: Same transform as last one, dropped.");
return;
}
last_transform_stamp = stamp;
auto position = ftf::transform_frame_enu_ned(Eigen::Vector3d(tr.translation()));
auto rpy = ftf::quaternion_to_rpy(
ftf::transform_orientation_enu_ned(Eigen::Quaterniond(tr.rotation())));
vision_position_estimate(stamp.toNSec() / 1000, position, rpy);
}
/**
* Send vision estimate transform to FCU position controller
*/
void send_vision_transform(const tf::Transform &transform, const ros::Time &stamp) {
// origin and RPY in ENU frame
tf::Vector3 position = transform.getOrigin();
double roll, pitch, yaw;
tf::Matrix3x3 orientation(transform.getBasis());
orientation.getRPY(roll, pitch, yaw);
/* Issue #60.
* Note: this now affects pose callbacks too, but i think its not big deal.
*/
if (last_transform_stamp == stamp) {
ROS_DEBUG_THROTTLE_NAMED(10, "position", "Vision: Same transform as last one, dropped.");
return;
}
last_transform_stamp = stamp;
// TODO: check conversion. Issue #49.
vision_position_estimate(stamp.toNSec() / 1000,
position.y(), position.x(), -position.z(),
roll, -pitch, -yaw); // ??? please check!
}
// Did you really need service?
// I think topic subscriber are better for this case
void offboard_vision_cb(const mavros_vision::OffboardVision &req) {
vision_position_estimate(req.x, req.y, req.z, req.roll, req.pitch, req.yaw);
}