gazebo_odometry_plugin.h
Go to the documentation of this file.
1 /*
2  * Copyright 2015 Fadri Furrer, ASL, ETH Zurich, Switzerland
3  * Copyright 2015 Michael Burri, ASL, ETH Zurich, Switzerland
4  * Copyright 2015 Mina Kamel, ASL, ETH Zurich, Switzerland
5  * Copyright 2015 Janosch Nikolic, ASL, ETH Zurich, Switzerland
6  * Copyright 2015 Markus Achtelik, ASL, ETH Zurich, Switzerland
7  * Copyright 2016 Geoffrey Hunter <gbmhunter@gmail.com>
8  *
9  * Licensed under the Apache License, Version 2.0 (the "License");
10  * you may not use this file except in compliance with the License.
11  * You may obtain a copy of the License at
12  *
13  * http://www.apache.org/licenses/LICENSE-2.0
14 
15  * Unless required by applicable law or agreed to in writing, software
16  * distributed under the License is distributed on an "AS IS" BASIS,
17  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
18  * See the License for the specific language governing permissions and
19  * limitations under the License.
20  */
21 
22 
23 #ifndef ROTORS_GAZEBO_PLUGINS_GAZEBO_ODOMETRY_PLUGIN_H
24 #define ROTORS_GAZEBO_PLUGINS_GAZEBO_ODOMETRY_PLUGIN_H
25 
26 #include <cmath>
27 #include <deque>
28 #include <random>
29 #include <stdio.h>
30 
31 #include <boost/array.hpp>
32 #include <boost/bind.hpp>
33 #include <gazebo/common/common.hh>
34 #include <gazebo/common/Plugin.hh>
35 #include <gazebo/gazebo.hh>
36 #include <gazebo/physics/physics.hh>
37 #include <opencv2/core/core.hpp>
38 
39 #include <mav_msgs/default_topics.h> // This comes from the mav_comm repo
40 
43 
44 #include "Odometry.pb.h"
45 
46 
47 namespace gazebo {
48 
49 // Default values
50 static const std::string kDefaultParentFrameId = "world";
51 static const std::string kDefaultChildFrameId = "odometry_sensor";
52 static const std::string kDefaultLinkName = "odometry_sensor_link";
53 
54 static constexpr int kDefaultMeasurementDelay = 0;
55 static constexpr int kDefaultMeasurementDivisor = 1;
56 static constexpr int kDefaultGazeboSequence = 0;
57 static constexpr int kDefaultOdometrySequence = 0;
58 static constexpr double kDefaultUnknownDelay = 0.0;
59 static constexpr double kDefaultCovarianceImageScale = 1.0;
60 
61 class GazeboOdometryPlugin : public ModelPlugin {
62  public:
63  typedef std::normal_distribution<> NormalDistribution;
64  typedef std::uniform_real_distribution<> UniformDistribution;
65  typedef std::deque<std::pair<int, gz_geometry_msgs::Odometry> > OdometryQueue;
66  typedef boost::array<double, 36> CovarianceMatrix;
67 
69  : ModelPlugin(),
71  // DEFAULT TOPICS
72  pose_pub_topic_(mav_msgs::default_topics::POSE),
73  pose_with_covariance_stamped_pub_topic_(mav_msgs::default_topics::POSE_WITH_COVARIANCE),
74  position_stamped_pub_topic_(mav_msgs::default_topics::POSITION),
75  transform_stamped_pub_topic_(mav_msgs::default_topics::TRANSFORM),
76  odometry_pub_topic_(mav_msgs::default_topics::ODOMETRY),
77  //---------------
78  parent_frame_id_(kDefaultParentFrameId),
79  child_frame_id_(kDefaultChildFrameId),
80  link_name_(kDefaultLinkName),
81  measurement_delay_(kDefaultMeasurementDelay),
82  measurement_divisor_(kDefaultMeasurementDivisor),
83  unknown_delay_(kDefaultUnknownDelay),
84  gazebo_sequence_(kDefaultGazeboSequence),
85  odometry_sequence_(kDefaultOdometrySequence),
86  covariance_image_scale_(kDefaultCovarianceImageScale),
87  pubs_and_subs_created_(false) {}
88 
90 
91  void InitializeParams();
92  void Publish();
93 
94  protected:
95  void Load(physics::ModelPtr _model, sdf::ElementPtr _sdf);
96  void OnUpdate(const common::UpdateInfo& /*_info*/);
97 
98  private:
99 
103 
108  void CreatePubsAndSubs();
109 
110  OdometryQueue odometry_queue_;
111 
112  std::string namespace_;
113  std::string pose_pub_topic_;
117  std::string odometry_pub_topic_;
118  std::string parent_frame_id_;
119  std::string child_frame_id_;
120  std::string link_name_;
121 
122  NormalDistribution position_n_[3];
123  NormalDistribution attitude_n_[3];
124  NormalDistribution linear_velocity_n_[3];
125  NormalDistribution angular_velocity_n_[3];
126  UniformDistribution position_u_[3];
127  UniformDistribution attitude_u_[3];
128  UniformDistribution linear_velocity_u_[3];
129  UniformDistribution angular_velocity_u_[3];
130 
131  CovarianceMatrix pose_covariance_matrix_;
132  CovarianceMatrix twist_covariance_matrix_;
133 
141 
142  std::random_device random_device_;
143  std::mt19937 random_generator_;
144 
145  gazebo::transport::NodePtr node_handle_;
146 
147  gazebo::transport::PublisherPtr pose_pub_;
148  gazebo::transport::PublisherPtr pose_with_covariance_stamped_pub_;
149  gazebo::transport::PublisherPtr position_stamped_pub_;
150  gazebo::transport::PublisherPtr transform_stamped_pub_;
151  gazebo::transport::PublisherPtr odometry_pub_;
152 
157  gazebo::transport::PublisherPtr broadcast_transform_pub_;
158 
159  physics::WorldPtr world_;
160  physics::ModelPtr model_;
161  physics::LinkPtr link_;
162  physics::EntityPtr parent_link_;
163 
165  event::ConnectionPtr updateConnection_;
166 
167  boost::thread callback_queue_thread_;
168  void QueueThread();
169 };
170 
171 } // namespace gazebo
172 
173 #endif // ROTORS_GAZEBO_PLUGINS_GAZEBO_ODOMETRY_PLUGIN_H
static const std::string kDefaultLinkName
UniformDistribution angular_velocity_u_[3]
gazebo::transport::PublisherPtr position_stamped_pub_
gazebo::transport::PublisherPtr transform_stamped_pub_
gazebo::transport::PublisherPtr odometry_pub_
UniformDistribution position_u_[3]
gazebo::transport::PublisherPtr pose_pub_
UniformDistribution linear_velocity_u_[3]
static constexpr double kDefaultUnknownDelay
static constexpr int kDefaultGazeboSequence
bool pubs_and_subs_created_
Flag that is set to true once CreatePubsAndSubs() is called, used to prevent CreatePubsAndSubs() from...
static const std::string kDefaultChildFrameId
NormalDistribution linear_velocity_n_[3]
event::ConnectionPtr updateConnection_
Pointer to the update event connection.
static constexpr double kDefaultCovarianceImageScale
std::deque< std::pair< int, gz_geometry_msgs::Odometry > > OdometryQueue
static constexpr int kDefaultMeasurementDivisor
static constexpr int kDefaultMeasurementDelay
gazebo::transport::PublisherPtr pose_with_covariance_stamped_pub_
void Load(physics::ModelPtr _model, sdf::ElementPtr _sdf)
NormalDistribution angular_velocity_n_[3]
void CreatePubsAndSubs()
Creates all required publishers and subscribers, incl. routing of messages to/from ROS if required...
UniformDistribution attitude_u_[3]
std::normal_distribution NormalDistribution
static constexpr int kDefaultOdometrySequence
boost::array< double, 36 > CovarianceMatrix
void OnUpdate(const common::UpdateInfo &)
gazebo::transport::NodePtr node_handle_
static const std::string kDefaultParentFrameId
std::uniform_real_distribution UniformDistribution
gazebo::transport::PublisherPtr broadcast_transform_pub_
Special-case publisher to publish stamped transforms with frame IDs. The ROS interface plugin (if pre...


rotors_gazebo_plugins
Author(s): Fadri Furrer, Michael Burri, Mina Kamel, Janosch Nikolic, Markus Achtelik
autogenerated on Mon Feb 28 2022 23:39:04