setpoint_accel.cpp
Go to the documentation of this file.
00001 
00010 /*
00011  * Copyright 2014 Nuno Marques.
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/utils.h>
00019 #include <mavros/mavros_plugin.h>
00020 #include <mavros/setpoint_mixin.h>
00021 #include <eigen_conversions/eigen_msg.h>
00022 #include <pluginlib/class_list_macros.h>
00023 
00024 #include <geometry_msgs/Vector3Stamped.h>
00025 
00026 namespace mavplugin {
00032 class SetpointAccelerationPlugin : public MavRosPlugin,
00033         private SetPositionTargetLocalNEDMixin<SetpointAccelerationPlugin> {
00034 public:
00035         SetpointAccelerationPlugin() :
00036                 sp_nh("~setpoint_accel"),
00037                 uas(nullptr),
00038                 send_force(false)
00039         { };
00040 
00041         void initialize(UAS &uas_)
00042         {
00043                 uas = &uas_;
00044 
00045                 sp_nh.param("send_force", send_force, false);
00046 
00047                 accel_sub = sp_nh.subscribe("accel", 10, &SetpointAccelerationPlugin::accel_cb, this);
00048         }
00049 
00050         const message_map get_rx_handlers() {
00051                 return { /* Rx disabled */ };
00052         }
00053 
00054 private:
00055         friend class SetPositionTargetLocalNEDMixin;
00056         ros::NodeHandle sp_nh;
00057         UAS *uas;
00058 
00059         ros::Subscriber accel_sub;
00060 
00061         bool send_force;
00062 
00063         /* -*- mid-level helpers -*- */
00064 
00070         void send_setpoint_acceleration(const ros::Time &stamp, Eigen::Vector3d &accel_enu) {
00071                 /* Documentation start from bit 1 instead 0.
00072                  * Ignore position and velocity vectors, yaw and yaw rate
00073                  */
00074                 uint16_t ignore_all_except_a_xyz = (3 << 10) | (7 << 3) | (7 << 0);
00075 
00076                 if (send_force)
00077                         ignore_all_except_a_xyz |= (1 << 9);
00078 
00079                 auto accel = UAS::transform_frame_enu_ned(accel_enu);
00080 
00081                 set_position_target_local_ned(stamp.toNSec() / 1000000,
00082                                 MAV_FRAME_LOCAL_NED,
00083                                 ignore_all_except_a_xyz,
00084                                 0.0, 0.0, 0.0,
00085                                 0.0, 0.0, 0.0,
00086                                 accel.x(), accel.y(), accel.z(),
00087                                 0.0, 0.0);
00088         }
00089 
00090         /* -*- callbacks -*- */
00091 
00092         void accel_cb(const geometry_msgs::Vector3Stamped::ConstPtr &req) {
00093                 Eigen::Vector3d accel_enu;
00094 
00095                 tf::vectorMsgToEigen(req->vector, accel_enu);
00096                 send_setpoint_acceleration(req->header.stamp, accel_enu);
00097         }
00098 };
00099 };      // namespace mavplugin
00100 
00101 PLUGINLIB_EXPORT_CLASS(mavplugin::SetpointAccelerationPlugin, mavplugin::MavRosPlugin)


mavros
Author(s): Vladimir Ermakov
autogenerated on Sat Jun 8 2019 19:55:19