vision_speed_estimate.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/mavros_plugin.h>
00019 #include <pluginlib/class_list_macros.h>
00020 #include <eigen_conversions/eigen_msg.h>
00021 
00022 #include <geometry_msgs/TwistStamped.h>
00023 #include <geometry_msgs/Vector3Stamped.h>
00024 
00025 namespace mavplugin {
00033 class VisionSpeedEstimatePlugin : public MavRosPlugin {
00034 public:
00035         VisionSpeedEstimatePlugin() :
00036                 sp_nh("~vision_speed"),
00037                 uas(nullptr)
00038         { };
00039 
00040         void initialize(UAS &uas_)
00041         {
00042                 bool listen_twist;
00043                 uas = &uas_;
00044 
00045                 sp_nh.param("listen_twist", listen_twist, false);
00046 
00047                 if (listen_twist)
00048                         vision_vel_sub = sp_nh.subscribe("speed_twist", 10, &VisionSpeedEstimatePlugin::vel_twist_cb, this);
00049                 else
00050                         vision_vel_sub = sp_nh.subscribe("speed_vector", 10, &VisionSpeedEstimatePlugin::vel_speed_cb, this);
00051         }
00052 
00053         const message_map get_rx_handlers() {
00054                 return { /* Rx disabled */ };
00055         }
00056 
00057 private:
00058         ros::NodeHandle sp_nh;
00059         UAS *uas;
00060 
00061         ros::Subscriber vision_vel_sub;
00062 
00063         /* -*- low-level send -*- */
00064 
00065         void vision_speed_estimate(uint64_t usec,
00066                         float x, float y, float z) {
00067                 mavlink_message_t msg;
00068                 mavlink_msg_vision_speed_estimate_pack_chan(UAS_PACK_CHAN(uas), &msg,
00069                                 usec,
00070                                 x, y, z);
00071                 UAS_FCU(uas)->send_message(&msg);
00072         }
00073 
00079         /* -*- mid-level helpers -*- */
00080 
00084         void send_vision_speed(const geometry_msgs::Vector3 &vel_enu, const ros::Time &stamp) {
00085                 Eigen::Vector3d vel_;
00086                 tf::vectorMsgToEigen(vel_enu, vel_);
00087                 //Transform from ENU to NED frame
00088                 auto vel = UAS::transform_frame_enu_ned(vel_);
00089 
00090                 vision_speed_estimate(stamp.toNSec() / 1000,
00091                                 vel.x(), vel.y(), vel.z());
00092         }
00093 
00094         /* -*- callbacks -*- */
00095 
00096         void vel_twist_cb(const geometry_msgs::TwistStamped::ConstPtr &req) {
00097                 send_vision_speed(req->twist.linear, req->header.stamp);
00098         }
00099 
00100         void vel_speed_cb(const geometry_msgs::Vector3Stamped::ConstPtr &req) {
00101                 send_vision_speed(req->vector, req->header.stamp);
00102         }
00103 };
00104 };      // namespace mavplugin
00105 
00106 PLUGINLIB_EXPORT_CLASS(mavplugin::VisionSpeedEstimatePlugin, mavplugin::MavRosPlugin)


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