gazebo_ros_imu.cpp
Go to the documentation of this file.
1 /*
2  * Copyright 2013 Open Source Robotics Foundation
3  *
4  * Licensed under the Apache License, Version 2.0 (the "License");
5  * you may not use this file except in compliance with the License.
6  * You may obtain a copy of the License at
7  *
8  * http://www.apache.org/licenses/LICENSE-2.0
9  *
10  * Unless required by applicable law or agreed to in writing, software
11  * distributed under the License is distributed on an "AS IS" BASIS,
12  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
13  * See the License for the specific language governing permissions and
14  * limitations under the License.
15  *
16 */
17 /*
18  * Desc: 3D position interface for ground truth.
19  * Author: Sachin Chitta and John Hsu
20  * Date: 1 June 2008
21  */
22 
24 #include <ignition/math/Rand.hh>
25 
26 namespace gazebo
27 {
28 // Register this plugin with the simulator
29 GZ_REGISTER_MODEL_PLUGIN(GazeboRosIMU)
30 
31 // Constructor
34 {
35 }
36 
38 // Destructor
40 {
41  this->update_connection_.reset();
42  // Finalize the controller
43  this->rosnode_->shutdown();
44  this->callback_queue_thread_.join();
45  delete this->rosnode_;
46 }
47 
49 // Load the controller
50 void GazeboRosIMU::Load(physics::ModelPtr _parent, sdf::ElementPtr _sdf)
51 {
52  // save pointers
53  this->world_ = _parent->GetWorld();
54  this->sdf = _sdf;
55 
56  // ros callback queue for processing subscription
57  this->deferred_load_thread_ = boost::thread(
58  boost::bind(&GazeboRosIMU::LoadThread, this));
59 }
60 
62 // Load the controller
64 {
65  // load parameters
66  this->robot_namespace_ = "";
67  if (this->sdf->HasElement("robotNamespace"))
68  this->robot_namespace_ = this->sdf->Get<std::string>("robotNamespace") + "/";
69 
70  if (!this->sdf->HasElement("serviceName"))
71  {
72  ROS_INFO_NAMED("imu", "imu plugin missing <serviceName>, defaults to /default_imu");
73  this->service_name_ = "/default_imu";
74  }
75  else
76  this->service_name_ = this->sdf->Get<std::string>("serviceName");
77 
78  if (!this->sdf->HasElement("topicName"))
79  {
80  ROS_INFO_NAMED("imu", "imu plugin missing <topicName>, defaults to /default_imu");
81  this->topic_name_ = "/default_imu";
82  }
83  else
84  this->topic_name_ = this->sdf->Get<std::string>("topicName");
85 
86  if (!this->sdf->HasElement("gaussianNoise"))
87  {
88  ROS_INFO_NAMED("imu", "imu plugin missing <gaussianNoise>, defaults to 0.0");
89  this->gaussian_noise_ = 0.0;
90  }
91  else
92  this->gaussian_noise_ = this->sdf->Get<double>("gaussianNoise");
93 
94  if (!this->sdf->HasElement("bodyName"))
95  {
96  ROS_FATAL_NAMED("imu", "imu plugin missing <bodyName>, cannot proceed");
97  return;
98  }
99  else
100  this->link_name_ = this->sdf->Get<std::string>("bodyName");
101 
102  if (!this->sdf->HasElement("xyzOffset"))
103  {
104  ROS_INFO_NAMED("imu", "imu plugin missing <xyzOffset>, defaults to 0s");
105  this->offset_.Pos() = ignition::math::Vector3d(0, 0, 0);
106  }
107  else
108  this->offset_.Pos() = this->sdf->Get<ignition::math::Vector3d>("xyzOffset");
109 
110  if (!this->sdf->HasElement("rpyOffset"))
111  {
112  ROS_INFO_NAMED("imu", "imu plugin missing <rpyOffset>, defaults to 0s");
113  this->offset_.Rot() = ignition::math::Quaterniond(ignition::math::Vector3d(0, 0, 0));
114  }
115  else
116  this->offset_.Rot() = ignition::math::Quaterniond(this->sdf->Get<ignition::math::Vector3d>("rpyOffset"));
117 
118  if (!this->sdf->HasElement("updateRate"))
119  {
120  ROS_DEBUG_NAMED("imu", "imu plugin missing <updateRate>, defaults to 0.0"
121  " (as fast as possible)");
122  this->update_rate_ = 0.0;
123  }
124  else
125  this->update_rate_ = this->sdf->GetElement("updateRate")->Get<double>();
126 
127  if (!this->sdf->HasElement("frameName"))
128  {
129  ROS_INFO_NAMED("imu", "imu plugin missing <frameName>, defaults to <bodyName>");
130  this->frame_name_ = link_name_;
131  }
132  else
133  this->frame_name_ = this->sdf->Get<std::string>("frameName");
134 
135  // Make sure the ROS node for Gazebo has already been initialized
136  if (!ros::isInitialized())
137  {
138  ROS_FATAL_STREAM_NAMED("imu", "A ROS node for Gazebo has not been initialized, unable to load plugin. "
139  << "Load the Gazebo system plugin 'libgazebo_ros_api_plugin.so' in the gazebo_ros package)");
140  return;
141  }
142 
143  this->rosnode_ = new ros::NodeHandle(this->robot_namespace_);
144 
145  // publish multi queue
146  this->pmq.startServiceThread();
147 
148  // assert that the body by link_name_ exists
149  this->link = boost::dynamic_pointer_cast<physics::Link>(
150 #if GAZEBO_MAJOR_VERSION >= 8
151  this->world_->EntityByName(this->link_name_));
152 #else
153  this->world_->GetEntity(this->link_name_));
154 #endif
155  if (!this->link)
156  {
157  ROS_FATAL_NAMED("imu", "gazebo_ros_imu plugin error: bodyName: %s does not exist\n",
158  this->link_name_.c_str());
159  return;
160  }
161 
162  // if topic name specified as empty, do not publish
163  if (this->topic_name_ != "")
164  {
165  this->pub_Queue = this->pmq.addPub<sensor_msgs::Imu>();
166  this->pub_ = this->rosnode_->advertise<sensor_msgs::Imu>(
167  this->topic_name_, 1);
168 
169  // advertise services on the custom queue
171  ros::AdvertiseServiceOptions::create<std_srvs::Empty>(
172  this->service_name_, boost::bind(&GazeboRosIMU::ServiceCallback,
173  this, _1, _2), ros::VoidPtr(), &this->imu_queue_);
174  this->srv_ = this->rosnode_->advertiseService(aso);
175  }
176 
177  // Initialize the controller
178 #if GAZEBO_MAJOR_VERSION >= 8
179  this->last_time_ = this->world_->SimTime();
180 #else
181  this->last_time_ = this->world_->GetSimTime();
182 #endif
183 
184  // this->initial_pose_ = this->link->GetPose();
185 #if GAZEBO_MAJOR_VERSION >= 8
186  this->last_vpos_ = this->link->WorldLinearVel();
187  this->last_veul_ = this->link->WorldAngularVel();
188 #else
189  this->last_vpos_ = this->link->GetWorldLinearVel().Ign();
190  this->last_veul_ = this->link->GetWorldAngularVel().Ign();
191 #endif
192  this->apos_ = 0;
193  this->aeul_ = 0;
194 
195  // start custom queue for imu
196  this->callback_queue_thread_ =
197  boost::thread(boost::bind(&GazeboRosIMU::IMUQueueThread, this));
198 
199 
200  // New Mechanism for Updating every World Cycle
201  // Listen to the update event. This event is broadcast every
202  // simulation iteration.
203  this->update_connection_ = event::Events::ConnectWorldUpdateBegin(
204  boost::bind(&GazeboRosIMU::UpdateChild, this));
205 }
206 
208 // returns true always, imu is always calibrated in sim
209 bool GazeboRosIMU::ServiceCallback(std_srvs::Empty::Request &req,
210  std_srvs::Empty::Response &res)
211 {
212  return true;
213 }
214 
216 // Update the controller
218 {
219 #if GAZEBO_MAJOR_VERSION >= 8
220  common::Time cur_time = this->world_->SimTime();
221 #else
222  common::Time cur_time = this->world_->GetSimTime();
223 #endif
224 
225  // rate control
226  if (this->update_rate_ > 0 &&
227  (cur_time - this->last_time_).Double() < (1.0 / this->update_rate_))
228  return;
229 
230  if ((this->pub_.getNumSubscribers() > 0 && this->topic_name_ != ""))
231  {
232  ignition::math::Pose3d pose;
233  ignition::math::Quaterniond rot;
234  ignition::math::Vector3d pos;
235 
236  // Get Pose/Orientation ///@todo: verify correctness
237 #if GAZEBO_MAJOR_VERSION >= 8
238  pose = this->link->WorldPose();
239 #else
240  pose = this->link->GetWorldPose().Ign();
241 #endif
242  // apply xyz offsets and get position and rotation components
243  pos = pose.Pos() + this->offset_.Pos();
244  rot = pose.Rot();
245 
246  // apply rpy offsets
247  rot = this->offset_.Rot()*rot;
248  rot.Normalize();
249 
250  // get Rates
251 #if GAZEBO_MAJOR_VERSION >= 8
252  ignition::math::Vector3d vpos = this->link->WorldLinearVel();
253  ignition::math::Vector3d veul = this->link->WorldAngularVel();
254 #else
255  ignition::math::Vector3d vpos = this->link->GetWorldLinearVel().Ign();
256  ignition::math::Vector3d veul = this->link->GetWorldAngularVel().Ign();
257 #endif
258 
259  // differentiate to get accelerations
260  double tmp_dt = this->last_time_.Double() - cur_time.Double();
261  if (tmp_dt != 0)
262  {
263  this->apos_ = (this->last_vpos_ - vpos) / tmp_dt;
264  this->aeul_ = (this->last_veul_ - veul) / tmp_dt;
265  this->last_vpos_ = vpos;
266  this->last_veul_ = veul;
267  }
268 
269  // copy data into pose message
270  this->imu_msg_.header.frame_id = this->frame_name_;
271  this->imu_msg_.header.stamp.sec = cur_time.sec;
272  this->imu_msg_.header.stamp.nsec = cur_time.nsec;
273 
274  // orientation quaternion
275 
276  // uncomment this if we are reporting orientation in the local frame
277  // not the case for our imu definition
278  // // apply fixed orientation offsets of initial pose
279  // rot = this->initial_pose_.Rot()*rot;
280  // rot.Normalize();
281 
282  this->imu_msg_.orientation.x = rot.X();
283  this->imu_msg_.orientation.y = rot.Y();
284  this->imu_msg_.orientation.z = rot.Z();
285  this->imu_msg_.orientation.w = rot.W();
286 
287  // pass euler angular rates
288  ignition::math::Vector3d linear_velocity(
289  veul.X() + this->GaussianKernel(0, this->gaussian_noise_),
290  veul.Y() + this->GaussianKernel(0, this->gaussian_noise_),
291  veul.Z() + this->GaussianKernel(0, this->gaussian_noise_));
292  // rotate into local frame
293  // @todo: deal with offsets!
294  linear_velocity = rot.RotateVector(linear_velocity);
295  this->imu_msg_.angular_velocity.x = linear_velocity.X();
296  this->imu_msg_.angular_velocity.y = linear_velocity.Y();
297  this->imu_msg_.angular_velocity.z = linear_velocity.Z();
298 
299  // pass accelerations
300  ignition::math::Vector3d linear_acceleration(
301  apos_.X() + this->GaussianKernel(0, this->gaussian_noise_),
302  apos_.Y() + this->GaussianKernel(0, this->gaussian_noise_),
303  apos_.Z() + this->GaussianKernel(0, this->gaussian_noise_));
304  // rotate into local frame
305  // @todo: deal with offsets!
306  linear_acceleration = rot.RotateVector(linear_acceleration);
307  this->imu_msg_.linear_acceleration.x = linear_acceleration.X();
308  this->imu_msg_.linear_acceleration.y = linear_acceleration.Y();
309  this->imu_msg_.linear_acceleration.z = linear_acceleration.Z();
310 
311  // fill in covariance matrix
314  double gn2 = this->gaussian_noise_*this->gaussian_noise_;
315  this->imu_msg_.orientation_covariance[0] = gn2;
316  this->imu_msg_.orientation_covariance[4] = gn2;
317  this->imu_msg_.orientation_covariance[8] = gn2;
318  this->imu_msg_.angular_velocity_covariance[0] = gn2;
319  this->imu_msg_.angular_velocity_covariance[4] = gn2;
320  this->imu_msg_.angular_velocity_covariance[8] = gn2;
321  this->imu_msg_.linear_acceleration_covariance[0] = gn2;
322  this->imu_msg_.linear_acceleration_covariance[4] = gn2;
323  this->imu_msg_.linear_acceleration_covariance[8] = gn2;
324 
325  {
326  boost::mutex::scoped_lock lock(this->lock_);
327  // publish to ros
328  if (this->pub_.getNumSubscribers() > 0 && this->topic_name_ != "")
329  this->pub_Queue->push(this->imu_msg_, this->pub_);
330  }
331 
332  // save last time stamp
333  this->last_time_ = cur_time;
334  }
335 }
336 
337 
339 // Utility for adding noise
340 double GazeboRosIMU::GaussianKernel(double mu, double sigma)
341 {
342  // using Box-Muller transform to generate two independent standard
343  // normally disbributed normal variables see wikipedia
344 
345  // normalized uniform random variable
346  double U = ignition::math::Rand::DblUniform();
347 
348  // normalized uniform random variable
349  double V = ignition::math::Rand::DblUniform();
350 
351  double X = sqrt(-2.0 * ::log(U)) * cos(2.0*M_PI * V);
352  // double Y = sqrt(-2.0 * ::log(U)) * sin(2.0*M_PI * V);
353 
354  // there are 2 indep. vars, we'll just use X
355  // scale to our mu and sigma
356  X = sigma * X + mu;
357  return X;
358 }
359 
361 // Put laser data to the interface
363 {
364  static const double timeout = 0.01;
365 
366  while (this->rosnode_->ok())
367  {
369  }
370 }
371 }
sensor_msgs::Imu imu_msg_
ros message
sdf::ElementPtr sdf
void push(T &msg, ros::Publisher &pub)
Push a new message onto the queue.
Definition: PubQueue.h:73
boost::mutex lock_
A mutex to lock access to fields that are used in message callbacks.
ignition::math::Vector3d last_veul_
#define ROS_INFO_NAMED(name,...)
ros::Publisher pub_
common::Time last_time_
save last_time
ros::ServiceServer srv_
double gaussian_noise_
Gaussian noise.
ROSCPP_DECL bool isInitialized()
void Load(physics::ModelPtr _parent, sdf::ElementPtr _sdf)
Load the controller.
ServiceServer advertiseService(const std::string &service, bool(T::*srv_func)(MReq &, MRes &), T *obj)
std::string frame_name_
store frame name
ignition::math::Vector3d last_vpos_
ignition::math::Vector3d apos_
std::string link_name_
store link name
#define ROS_DEBUG_NAMED(name,...)
GZ_REGISTER_MODEL_PLUGIN(GazeboRosF3D)
boost::thread deferred_load_thread_
boost::thread callback_queue_thread_
#define ROS_FATAL_STREAM_NAMED(name, args)
std::string topic_name_
topic name
virtual void UpdateChild()
Update the controller.
virtual ~GazeboRosIMU()
Destructor.
physics::WorldPtr world_
The parent World.
ignition::math::Vector3d aeul_
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
ros::NodeHandle * rosnode_
pointer to ros node
boost::shared_ptr< PubQueue< T > > addPub()
Add a new queue. Call this once for each published topic (or at least each type of publish message)...
Definition: PubQueue.h:143
event::ConnectionPtr update_connection_
std::string service_name_
std::string robot_namespace_
for setting ROS name space
ros::CallbackQueue imu_queue_
ignition::math::Pose3d offset_
allow specifying constant xyz and rpy offsets
physics::LinkPtr link
The link referred to by this plugin.
#define ROS_FATAL_NAMED(name,...)
PubQueue< sensor_msgs::Imu >::Ptr pub_Queue
void startServiceThread()
Start a thread to call spin().
Definition: PubQueue.h:181
double GaussianKernel(double mu, double sigma)
Gaussian noise generator.
uint32_t getNumSubscribers() const
bool ok() const
boost::shared_ptr< void > VoidPtr
bool ServiceCallback(std_srvs::Empty::Request &req, std_srvs::Empty::Response &res)
call back when using service


gazebo_plugins
Author(s): John Hsu
autogenerated on Wed Aug 24 2022 02:47:52