mocap_pose_estimate.cpp
Go to the documentation of this file.
1 
10 /*
11  * Copyright 2014,2015,2016 Vladimir Ermakov, Tony Baltovski.
12  *
13  * This file is part of the mavros package and subject to the license terms
14  * in the top-level LICENSE file of the mavros repository.
15  * https://github.com/mavlink/mavros/tree/master/LICENSE.md
16  */
17 
18 #include <mavros/mavros_plugin.h>
20 
21 #include <geometry_msgs/PoseStamped.h>
22 #include <geometry_msgs/TransformStamped.h>
23 
24 
25 namespace mavros {
26 namespace extra_plugins{
33 {
34 public:
36  mp_nh("~mocap")
37  { }
38 
39  void initialize(UAS &uas_) override
40  {
42 
43  bool use_tf;
44  bool use_pose;
45 
47  mp_nh.param("use_tf", use_tf, false);
48 
50  mp_nh.param("use_pose", use_pose, true);
51 
52  if (use_tf && !use_pose) {
54  }
55  else if (use_pose && !use_tf) {
57  }
58  else {
59  ROS_ERROR_NAMED("mocap", "Use one motion capture source.");
60  }
61  }
62 
64  {
65  return { /* Rx disabled */ };
66  }
67 
68 private:
70 
73 
74  /* -*- low-level send -*- */
75  void mocap_pose_send
76  (uint64_t usec,
77  Eigen::Quaterniond &q,
78  Eigen::Vector3d &v)
79  {
80  mavlink::common::msg::ATT_POS_MOCAP pos = {};
81 
82  pos.time_usec = usec;
84  pos.x = v.x();
85  pos.y = v.y();
86  pos.z = v.z();
87 
88  UAS_FCU(m_uas)->send_message_ignore_drop(pos);
89  }
90 
91  /* -*- mid-level helpers -*- */
93  {
94  Eigen::Quaterniond q_enu;
95 
96  tf::quaternionMsgToEigen(pose->pose.orientation, q_enu);
99 
100  auto position = ftf::transform_frame_enu_ned(
101  Eigen::Vector3d(
102  pose->pose.position.x,
103  pose->pose.position.y,
104  pose->pose.position.z));
105 
106  mocap_pose_send(pose->header.stamp.toNSec() / 1000,
107  q,
108  position);
109  }
110 
111  /* -*- callbacks -*- */
113  {
114  Eigen::Quaterniond q_enu;
115 
116  tf::quaternionMsgToEigen(trans->transform.rotation, q_enu);
119 
120  auto position = ftf::transform_frame_enu_ned(
121  Eigen::Vector3d(
122  trans->transform.translation.x,
123  trans->transform.translation.y,
124  trans->transform.translation.z));
125 
126  mocap_pose_send(trans->header.stamp.toNSec() / 1000,
127  q,
128  position);
129  }
130 };
131 } // namespace extra_plugins
132 } // namespace mavros
133 
std::shared_ptr< MAVConnInterface const > ConstPtr
void mocap_tf_cb(const geometry_msgs::TransformStamped::ConstPtr &trans)
ssize_t pos
Subscriber subscribe(const std::string &topic, uint32_t queue_size, void(T::*fp)(M), T *obj, const TransportHints &transport_hints=TransportHints())
void mocap_pose_send(uint64_t usec, Eigen::Quaterniond &q, Eigen::Vector3d &v)
void quaternion_to_mavlink(const Eigen::Quaternion< _Scalar > &q, std::array< float, 4 > &qmsg)
T transform_orientation_enu_ned(const T &in)
bool param(const std::string &param_name, T &param_val, const T &default_val) const
#define UAS_FCU(uasobjptr)
void mocap_pose_cb(const geometry_msgs::PoseStamped::ConstPtr &pose)
T transform_frame_enu_ned(const T &in)
std::vector< HandlerInfo > Subscriptions
void quaternionMsgToEigen(const geometry_msgs::Quaternion &m, Eigen::Quaterniond &e)
#define ROS_ERROR_NAMED(name,...)
T transform_orientation_baselink_aircraft(const T &in)
void initialize(UAS &uas_) override
#define PLUGINLIB_EXPORT_CLASS(class_type, base_class_type)


mavros_extras
Author(s): Vladimir Ermakov , Amilcar Lucas
autogenerated on Tue Jun 1 2021 02:36:36