local_position.cpp
Go to the documentation of this file.
00001 
00011 /*
00012  * Copyright 2014 Vladimir Ermakov.
00013  *
00014  * This file is part of the mavros package and subject to the license terms
00015  * in the top-level LICENSE file of the mavros repository.
00016  * https://github.com/mavlink/mavros/tree/master/LICENSE.md
00017  */
00018 
00019 #include <mavros/mavros_plugin.h>
00020 #include <pluginlib/class_list_macros.h>
00021 #include <eigen_conversions/eigen_msg.h>
00022 
00023 #include <geometry_msgs/PoseStamped.h>
00024 #include <geometry_msgs/TwistStamped.h>
00025 #include <geometry_msgs/TransformStamped.h>
00026 
00027 #include <nav_msgs/Odometry.h>
00028 
00029 namespace mavplugin {
00035 class LocalPositionPlugin : public MavRosPlugin {
00036 public:
00037         LocalPositionPlugin() :
00038                 lp_nh("~local_position"),
00039                 uas(nullptr),
00040                 tf_send(false)
00041         { };
00042 
00043         void initialize(UAS &uas_)
00044         {
00045                 uas = &uas_;
00046 
00047                 // header frame_id.
00048                 // default to map (world-fixed,ENU as per REP-105).
00049                 lp_nh.param<std::string>("frame_id", frame_id, "map");
00050                 // Important tf subsection
00051                 // Report the transform from world to base_link here.
00052                 lp_nh.param("tf/send", tf_send, true);
00053                 lp_nh.param<std::string>("tf/frame_id", tf_frame_id, "map");
00054                 lp_nh.param<std::string>("tf/child_frame_id", tf_child_frame_id, "base_link");
00055                 // Debug tf info
00056                 // broadcast the following transform: (can be expanded to more if desired)
00057                 // NED -> aircraft
00058                 lp_nh.param("tf/send_fcu",tf_send_fcu,false);
00059 
00060                 local_position = lp_nh.advertise<geometry_msgs::PoseStamped>("pose", 10);
00061                 local_velocity = lp_nh.advertise<geometry_msgs::TwistStamped>("velocity", 10);
00062                 local_odom = lp_nh.advertise<nav_msgs::Odometry>("odom",10);
00063         }
00064 
00065         const message_map get_rx_handlers() {
00066                 return {
00067                                MESSAGE_HANDLER(MAVLINK_MSG_ID_LOCAL_POSITION_NED, &LocalPositionPlugin::handle_local_position_ned)
00068                 };
00069         }
00070 
00071 private:
00072         ros::NodeHandle lp_nh;
00073         UAS *uas;
00074 
00075         ros::Publisher local_position;
00076         ros::Publisher local_velocity;
00077         ros::Publisher local_odom;
00078 
00079         std::string frame_id;           
00080         std::string tf_frame_id;        
00081         std::string tf_child_frame_id;  
00082         bool tf_send;
00083         bool tf_send_fcu;       
00084 
00085         void handle_local_position_ned(const mavlink_message_t *msg, uint8_t sysid, uint8_t compid) {
00086                 mavlink_local_position_ned_t pos_ned;
00087                 mavlink_msg_local_position_ned_decode(msg, &pos_ned);
00088 
00089                 //--------------- Transform FCU position and Velocity Data ---------------//
00090                 auto enu_position = UAS::transform_frame_ned_enu(Eigen::Vector3d(pos_ned.x, pos_ned.y, pos_ned.z));
00091                 auto enu_velocity = UAS::transform_frame_ned_enu(Eigen::Vector3d(pos_ned.vx, pos_ned.vy, pos_ned.vz));
00092 
00093                 //--------------- Get Odom Information ---------------//
00094                 // Note this orientation describes baselink->ENU transform
00095                 auto enu_orientation_msg = uas->get_attitude_orientation();
00096                 auto baselink_angular_msg = uas->get_attitude_angular_velocity();
00097                 Eigen::Quaterniond enu_orientation;
00098                 tf::quaternionMsgToEigen(enu_orientation_msg,enu_orientation);
00099                 auto baselink_linear = UAS::transform_frame_enu_baselink(enu_velocity,enu_orientation.inverse());
00100 
00101                 //--------------- Generate Message Pointers ---------------//
00102                 auto pose = boost::make_shared<geometry_msgs::PoseStamped>();
00103                 auto twist = boost::make_shared<geometry_msgs::TwistStamped>();
00104                 auto odom = boost::make_shared<nav_msgs::Odometry>();
00105 
00106                 pose->header = uas->synchronized_header(frame_id, pos_ned.time_boot_ms);
00107                 twist->header = pose->header;
00108 
00109                 tf::pointEigenToMsg(enu_position, pose->pose.position);
00110                 pose->pose.orientation = enu_orientation_msg;
00111 
00112                 tf::vectorEigenToMsg(enu_velocity,twist->twist.linear);
00113                 twist->twist.angular = baselink_angular_msg;
00114 
00115                 odom->header.stamp = pose->header.stamp;
00116                 odom->header.frame_id = tf_frame_id;
00117                 odom->child_frame_id = tf_child_frame_id;
00118                 tf::vectorEigenToMsg(baselink_linear,odom->twist.twist.linear);
00119                 odom->twist.twist.angular = baselink_angular_msg;
00120                 odom->pose.pose = pose->pose;
00121 
00122                 //--------------- Publish Data ---------------//
00123                 local_odom.publish(odom);
00124                 local_position.publish(pose);
00125                 local_velocity.publish(twist);
00126 
00127                 if (tf_send) {
00128                         geometry_msgs::TransformStamped transform;
00129 
00130                         transform.header.stamp = pose->header.stamp;
00131                         transform.header.frame_id = tf_frame_id;
00132                         transform.child_frame_id = tf_child_frame_id;
00133 
00134                         transform.transform.rotation = enu_orientation_msg;
00135                         tf::vectorEigenToMsg(enu_position, transform.transform.translation);
00136 
00137                         uas->tf2_broadcaster.sendTransform(transform);
00138                 }
00139                 if (tf_send_fcu) {
00140                         //--------------- Report NED->aircraft transform ---------------//
00141                         geometry_msgs::TransformStamped ned_aircraft_tf;
00142 
00143                         ned_aircraft_tf.header.stamp = pose->header.stamp;
00144                         ned_aircraft_tf.header.frame_id = "NED";
00145                         ned_aircraft_tf.child_frame_id = "aircraft";
00146 
00147                         //Don't just report the data from the mavlink message,
00148                         //actually perform rotations to see if anything is
00149                         //wrong.
00150                         auto ned_position = UAS::transform_frame_enu_ned(enu_position);
00151                         tf::vectorEigenToMsg(ned_position, ned_aircraft_tf.transform.translation);
00152 
00153                         auto ned_orientation = UAS::transform_orientation_enu_ned(
00154                                         UAS::transform_orientation_baselink_aircraft(enu_orientation));
00155                         tf::quaternionEigenToMsg(ned_orientation,ned_aircraft_tf.transform.rotation);
00156                         uas->tf2_broadcaster.sendTransform(ned_aircraft_tf);
00157                 }
00158         }
00159 };
00160 };      // namespace mavplugin
00161 
00162 PLUGINLIB_EXPORT_CLASS(mavplugin::LocalPositionPlugin, mavplugin::MavRosPlugin)
00163 
00164 


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