gazebo_odometry_plugin.h
Go to the documentation of this file.
00001 /*
00002  * Copyright 2015 Fadri Furrer, ASL, ETH Zurich, Switzerland
00003  * Copyright 2015 Michael Burri, ASL, ETH Zurich, Switzerland
00004  * Copyright 2015 Mina Kamel, ASL, ETH Zurich, Switzerland
00005  * Copyright 2015 Janosch Nikolic, ASL, ETH Zurich, Switzerland
00006  * Copyright 2015 Markus Achtelik, ASL, ETH Zurich, Switzerland
00007  * Copyright 2016 Geoffrey Hunter <gbmhunter@gmail.com>
00008  *
00009  * Licensed under the Apache License, Version 2.0 (the "License");
00010  * you may not use this file except in compliance with the License.
00011  * You may obtain a copy of the License at
00012  *
00013  *     http://www.apache.org/licenses/LICENSE-2.0
00014 
00015  * Unless required by applicable law or agreed to in writing, software
00016  * distributed under the License is distributed on an "AS IS" BASIS,
00017  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
00018  * See the License for the specific language governing permissions and
00019  * limitations under the License.
00020  */
00021 
00022 
00023 #ifndef ROTORS_GAZEBO_PLUGINS_GAZEBO_ODOMETRY_PLUGIN_H
00024 #define ROTORS_GAZEBO_PLUGINS_GAZEBO_ODOMETRY_PLUGIN_H
00025 
00026 #include <cmath>
00027 #include <deque>
00028 #include <random>
00029 #include <stdio.h>
00030 
00031 #include <boost/array.hpp>
00032 #include <boost/bind.hpp>
00033 #include <gazebo/common/common.hh>
00034 #include <gazebo/common/Plugin.hh>
00035 #include <gazebo/gazebo.hh>
00036 #include <gazebo/physics/physics.hh>
00037 #include <opencv2/core/core.hpp>
00038 
00039 #include <mav_msgs/default_topics.h>  // This comes from the mav_comm repo
00040 
00041 #include "rotors_gazebo_plugins/common.h"
00042 #include "rotors_gazebo_plugins/sdf_api_wrapper.hpp"
00043 
00044 #include "Odometry.pb.h"
00045 
00046 
00047 namespace gazebo {
00048 
00049 // Default values
00050 static const std::string kDefaultParentFrameId = "world";
00051 static const std::string kDefaultChildFrameId = "odometry_sensor";
00052 static const std::string kDefaultLinkName = "odometry_sensor_link";
00053 
00054 static constexpr int kDefaultMeasurementDelay = 0;
00055 static constexpr int kDefaultMeasurementDivisor = 1;
00056 static constexpr int kDefaultGazeboSequence = 0;
00057 static constexpr int kDefaultOdometrySequence = 0;
00058 static constexpr double kDefaultUnknownDelay = 0.0;
00059 static constexpr double kDefaultCovarianceImageScale = 1.0;
00060 
00061 class GazeboOdometryPlugin : public ModelPlugin {
00062  public:
00063   typedef std::normal_distribution<> NormalDistribution;
00064   typedef std::uniform_real_distribution<> UniformDistribution;
00065   typedef std::deque<std::pair<int, gz_geometry_msgs::Odometry> > OdometryQueue;
00066   typedef boost::array<double, 36> CovarianceMatrix;
00067 
00068   GazeboOdometryPlugin()
00069       : ModelPlugin(),
00070         random_generator_(random_device_()),
00071         // DEFAULT TOPICS
00072         pose_pub_topic_(mav_msgs::default_topics::POSE),
00073         pose_with_covariance_stamped_pub_topic_(mav_msgs::default_topics::POSE_WITH_COVARIANCE),
00074         position_stamped_pub_topic_(mav_msgs::default_topics::POSITION),
00075         transform_stamped_pub_topic_(mav_msgs::default_topics::TRANSFORM),
00076         odometry_pub_topic_(mav_msgs::default_topics::ODOMETRY),
00077         //---------------
00078         parent_frame_id_(kDefaultParentFrameId),
00079         child_frame_id_(kDefaultChildFrameId),
00080         link_name_(kDefaultLinkName),
00081         measurement_delay_(kDefaultMeasurementDelay),
00082         measurement_divisor_(kDefaultMeasurementDivisor),
00083         unknown_delay_(kDefaultUnknownDelay),
00084         gazebo_sequence_(kDefaultGazeboSequence),
00085         odometry_sequence_(kDefaultOdometrySequence),
00086         covariance_image_scale_(kDefaultCovarianceImageScale),
00087         pubs_and_subs_created_(false) {}
00088 
00089   ~GazeboOdometryPlugin();
00090 
00091   void InitializeParams();
00092   void Publish();
00093 
00094  protected:
00095   void Load(physics::ModelPtr _model, sdf::ElementPtr _sdf);
00096   void OnUpdate(const common::UpdateInfo& /*_info*/);
00097 
00098  private:
00099 
00102   bool pubs_and_subs_created_;
00103 
00108   void CreatePubsAndSubs();
00109 
00110   OdometryQueue odometry_queue_;
00111 
00112   std::string namespace_;
00113   std::string pose_pub_topic_;
00114   std::string pose_with_covariance_stamped_pub_topic_;
00115   std::string position_stamped_pub_topic_;
00116   std::string transform_stamped_pub_topic_;
00117   std::string odometry_pub_topic_;
00118   std::string parent_frame_id_;
00119   std::string child_frame_id_;
00120   std::string link_name_;
00121 
00122   NormalDistribution position_n_[3];
00123   NormalDistribution attitude_n_[3];
00124   NormalDistribution linear_velocity_n_[3];
00125   NormalDistribution angular_velocity_n_[3];
00126   UniformDistribution position_u_[3];
00127   UniformDistribution attitude_u_[3];
00128   UniformDistribution linear_velocity_u_[3];
00129   UniformDistribution angular_velocity_u_[3];
00130 
00131   CovarianceMatrix pose_covariance_matrix_;
00132   CovarianceMatrix twist_covariance_matrix_;
00133 
00134   int measurement_delay_;
00135   int measurement_divisor_;
00136   int gazebo_sequence_;
00137   int odometry_sequence_;
00138   double unknown_delay_;
00139   double covariance_image_scale_;
00140   cv::Mat covariance_image_;
00141 
00142   std::random_device random_device_;
00143   std::mt19937 random_generator_;
00144 
00145   gazebo::transport::NodePtr node_handle_;
00146 
00147   gazebo::transport::PublisherPtr pose_pub_;
00148   gazebo::transport::PublisherPtr pose_with_covariance_stamped_pub_;
00149   gazebo::transport::PublisherPtr position_stamped_pub_;
00150   gazebo::transport::PublisherPtr transform_stamped_pub_;
00151   gazebo::transport::PublisherPtr odometry_pub_;
00152 
00157   gazebo::transport::PublisherPtr broadcast_transform_pub_;
00158 
00159   physics::WorldPtr world_;
00160   physics::ModelPtr model_;
00161   physics::LinkPtr link_;
00162   physics::EntityPtr parent_link_;
00163 
00165   event::ConnectionPtr updateConnection_;
00166 
00167   boost::thread callback_queue_thread_;
00168   void QueueThread();
00169 };
00170 
00171 } // namespace gazebo
00172 
00173 #endif // ROTORS_GAZEBO_PLUGINS_GAZEBO_ODOMETRY_PLUGIN_H


rotors_gazebo_plugins
Author(s): Fadri Furrer, Michael Burri, Mina Kamel, Janosch Nikolic, Markus Achtelik
autogenerated on Thu Apr 18 2019 02:43:43