mocap_pose_estimate.cpp
Go to the documentation of this file.
00001 
00010 /*
00011  * Copyright 2014,2015 Vladimir Ermakov, Tony Baltovski.
00012  *
00013  * This file is part of the mavros package and subject to the license terms
00014  * in the top-level LICENSE file of the mavros repository.
00015  * https://github.com/mavlink/mavros/tree/master/LICENSE.md
00016  */
00017 
00018 #include <mavros/mavros_plugin.h>
00019 #include <pluginlib/class_list_macros.h>
00020 #include <eigen_conversions/eigen_msg.h>
00021 
00022 #include <geometry_msgs/PoseStamped.h>
00023 #include <geometry_msgs/TransformStamped.h>
00024 
00025 
00026 namespace mavplugin {
00032 class MocapPoseEstimatePlugin : public MavRosPlugin
00033 {
00034 public:
00035         MocapPoseEstimatePlugin() :
00036                 mp_nh("~mocap"),
00037                 uas(nullptr)
00038         { };
00039 
00040         void initialize(UAS &uas_)
00041         {
00042                 bool use_tf;
00043                 bool use_pose;
00044 
00045                 uas = &uas_;
00046 
00048                 mp_nh.param("use_tf", use_tf, false);
00049 
00051                 mp_nh.param("use_pose", use_pose, true);
00052 
00053                 if (use_tf && !use_pose) {
00054                         mocap_tf_sub = mp_nh.subscribe("tf", 1, &MocapPoseEstimatePlugin::mocap_tf_cb, this);
00055                 }
00056                 else if (use_pose && !use_tf) {
00057                         mocap_pose_sub = mp_nh.subscribe("pose", 1, &MocapPoseEstimatePlugin::mocap_pose_cb, this);
00058                 }
00059                 else {
00060                         ROS_ERROR_NAMED("mocap", "Use one motion capture source.");
00061                 }
00062         }
00063 
00064         const message_map get_rx_handlers() {
00065                 return { /* Rx disabled */ };
00066         }
00067 
00068 private:
00069         ros::NodeHandle mp_nh;
00070         UAS *uas;
00071 
00072         ros::Subscriber mocap_pose_sub;
00073         ros::Subscriber mocap_tf_sub;
00074 
00075         /* -*- low-level send -*- */
00076         void mocap_pose_send
00077                 (uint64_t usec,
00078                         float q[4],
00079                         float x, float y, float z)
00080         {
00081                 mavlink_message_t msg;
00082                 mavlink_msg_att_pos_mocap_pack_chan(UAS_PACK_CHAN(uas), &msg,
00083                                 usec,
00084                                 q,
00085                                 x,
00086                                 y,
00087                                 z);
00088                 UAS_FCU(uas)->send_message(&msg);
00089         }
00090 
00091         /* -*- mid-level helpers -*- */
00092         void mocap_pose_cb(const geometry_msgs::PoseStamped::ConstPtr &pose)
00093         {
00094                 Eigen::Quaterniond q_enu;
00095                 float q[4];
00096 
00097                 tf::quaternionMsgToEigen(pose->pose.orientation, q_enu);
00098                 UAS::quaternion_to_mavlink(
00099                                 UAS::transform_orientation_enu_ned(
00100                                         UAS::transform_orientation_baselink_aircraft(q_enu)),
00101                                 q);
00102 
00103                 auto position = UAS::transform_frame_enu_ned(
00104                                 Eigen::Vector3d(
00105                                         pose->pose.position.x,
00106                                         pose->pose.position.y,
00107                                         pose->pose.position.z));
00108 
00109                 mocap_pose_send(pose->header.stamp.toNSec() / 1000,
00110                                 q,
00111                                 position.x(),
00112                                 position.y(),
00113                                 position.z());
00114         }
00115 
00116         /* -*- callbacks -*- */
00117         void mocap_tf_cb(const geometry_msgs::TransformStamped::ConstPtr &trans)
00118         {
00119                 Eigen::Quaterniond q_enu;
00120                 float q[4];
00121 
00122                 tf::quaternionMsgToEigen(trans->transform.rotation, q_enu);
00123                 UAS::quaternion_to_mavlink(
00124                                 UAS::transform_orientation_enu_ned(
00125                                         UAS::transform_orientation_baselink_aircraft(q_enu)),
00126                                 q);
00127 
00128                 auto position = UAS::transform_frame_enu_ned(
00129                                 Eigen::Vector3d(
00130                                         trans->transform.translation.x,
00131                                         trans->transform.translation.y,
00132                                         trans->transform.translation.z));
00133 
00134                 mocap_pose_send(trans->header.stamp.toNSec() / 1000,
00135                                 q,
00136                                 position.x(),
00137                                 position.y(),
00138                                 position.z());
00139         }
00140 };
00141 };      // namespace mavplugin
00142 
00143 PLUGINLIB_EXPORT_CLASS(mavplugin::MocapPoseEstimatePlugin, mavplugin::MavRosPlugin)


mavros_extras
Author(s): Vladimir Ermakov
autogenerated on Thu Feb 9 2017 04:00:23