setpoint_mixin.h
Go to the documentation of this file.
00001 
00009 /*
00010  * Copyright 2014 Vladimir Ermakov.
00011  *
00012  * This file is part of the mavros package and subject to the license terms
00013  * in the top-level LICENSE file of the mavros repository.
00014  * https://github.com/mavlink/mavros/tree/master/LICENSE.md
00015  */
00016 
00017 #pragma once
00018 
00019 #include <mavros/utils.h>
00020 #include <mavros/mavros_plugin.h>
00021 
00022 #include <geometry_msgs/TransformStamped.h>
00023 
00024 namespace mavplugin {
00030 template <class D>
00031 class SetPositionTargetLocalNEDMixin {
00032 public:
00033         void set_position_target_local_ned(uint32_t time_boot_ms, uint8_t coordinate_frame,
00034                         uint16_t type_mask,
00035                         float x, float y, float z,
00036                         float vx, float vy, float vz,
00037                         float afx, float afy, float afz,
00038                         float yaw, float yaw_rate) {
00039                 UAS *_uas = static_cast<D *>(this)->uas;
00040                 mavlink_message_t msg;
00041                 mavlink_msg_set_position_target_local_ned_pack_chan(UAS_PACK_CHAN(_uas), &msg,
00042                                 time_boot_ms,   // why it not usec timestamp?
00043                                 UAS_PACK_TGT(_uas),
00044                                 coordinate_frame,
00045                                 type_mask,
00046                                 x, y, z,
00047                                 vx, vy, vz,
00048                                 afx, afy, afz,
00049                                 yaw, yaw_rate);
00050                 UAS_FCU(_uas)->send_message(&msg);
00051         }
00052 };
00053 
00060 template <class D>
00061 class TF2ListenerMixin {
00062 public:
00063         std::thread tf_thread;
00064         std::string tf_thd_name;
00065         boost::function<void (const geometry_msgs::TransformStamped &)> tf_transform_cb;
00066 
00073         void tf2_start(const char *_thd_name, void (D::*cbp)(const geometry_msgs::TransformStamped &) ) {
00074                 tf_thd_name = _thd_name;
00075                 tf_transform_cb = boost::bind(cbp, static_cast<D *>(this), _1);
00076 
00077                 std::thread t(boost::bind(&TF2ListenerMixin::tf_listener, this));
00078                 mavutils::set_thread_name(t, tf_thd_name);
00079                 tf_thread.swap(t);
00080         }
00081 
00082         void tf_listener(void) {
00083                 mavros::UAS *_uas = static_cast<D *>(this)->uas;
00084                 std::string &_frame_id = static_cast<D *>(this)->tf_frame_id;
00085                 std::string &_child_frame_id = static_cast<D *>(this)->tf_child_frame_id;
00086 
00087                 ros::Rate rate(static_cast<D *>(this)->tf_rate);
00088                 while (ros::ok()) {
00089                         // Wait up to 3s for transform
00090                         if (_uas->tf2_buffer.canTransform(_frame_id, _child_frame_id, ros::Time(0), ros::Duration(3.0))) {
00091                                 try {
00092                                         auto transform = _uas->tf2_buffer.lookupTransform(
00093                                                         _frame_id, _child_frame_id, ros::Time(0), ros::Duration(3.0));
00094                                         tf_transform_cb(transform);
00095                                 }
00096                                 catch (tf2::LookupException &ex) {
00097                                         ROS_ERROR_NAMED("tf2_buffer", "%s: %s", tf_thd_name.c_str(), ex.what());
00098                                 }
00099                         }
00100                         rate.sleep();
00101                 }
00102         }
00103 };
00104 };      // namespace mavplugin


mavros
Author(s): Vladimir Ermakov
autogenerated on Thu Feb 9 2017 04:00:17