Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
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>
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
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
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& );
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 }
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