Go to the documentation of this file.00001
00010
00011
00012
00013
00014
00015
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 { };
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
00064
00070 void send_setpoint_acceleration(const ros::Time &stamp, Eigen::Vector3d &accel_enu) {
00071
00072
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
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 };
00100
00101 PLUGINLIB_EXPORT_CLASS(mavplugin::SetpointAccelerationPlugin, mavplugin::MavRosPlugin)