gazebo_ros_p3d.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 #include <string>
19 #include <tf/tf.h>
20 #include <stdlib.h>
21 
23 
24 namespace gazebo
25 {
26 GZ_REGISTER_MODEL_PLUGIN(GazeboRosP3D);
27 
29 // Constructor
31 {
32  this->seed = 0;
33 }
34 
36 // Destructor
38 {
39  this->update_connection_.reset();
40  // Finalize the controller
41  this->rosnode_->shutdown();
42  this->p3d_queue_.clear();
43  this->p3d_queue_.disable();
44  this->callback_queue_thread_.join();
45  delete this->rosnode_;
46 }
47 
49 // Load the controller
50 void GazeboRosP3D::Load(physics::ModelPtr _parent, sdf::ElementPtr _sdf)
51 {
52  // Get the world name.
53  this->world_ = _parent->GetWorld();
54  this->model_ = _parent;
55 
56  // load parameters
57  this->robot_namespace_ = "";
58  if (_sdf->HasElement("robotNamespace"))
59  this->robot_namespace_ =
60  _sdf->GetElement("robotNamespace")->Get<std::string>() + "/";
61 
62  if (!_sdf->HasElement("bodyName"))
63  {
64  ROS_FATAL_NAMED("p3d", "p3d plugin missing <bodyName>, cannot proceed");
65  return;
66  }
67  else
68  this->link_name_ = _sdf->GetElement("bodyName")->Get<std::string>();
69 
70  this->link_ = _parent->GetLink(this->link_name_);
71  if (!this->link_)
72  {
73  ROS_FATAL_NAMED("p3d", "gazebo_ros_p3d plugin error: bodyName: %s does not exist\n",
74  this->link_name_.c_str());
75  return;
76  }
77 
78  if (!_sdf->HasElement("topicName"))
79  {
80  ROS_FATAL_NAMED("p3d", "p3d plugin missing <topicName>, cannot proceed");
81  return;
82  }
83  else
84  this->topic_name_ = _sdf->GetElement("topicName")->Get<std::string>();
85 
86  if (!_sdf->HasElement("frameName"))
87  {
88  ROS_DEBUG_NAMED("p3d", "p3d plugin missing <frameName>, defaults to world");
89  this->frame_name_ = "world";
90  }
91  else
92  this->frame_name_ = _sdf->GetElement("frameName")->Get<std::string>();
93 
94  if (!_sdf->HasElement("xyzOffset"))
95  {
96  ROS_DEBUG_NAMED("p3d", "p3d plugin missing <xyzOffset>, defaults to 0s");
97  this->offset_.Pos() = ignition::math::Vector3d(0, 0, 0);
98  }
99  else
100  this->offset_.Pos() = _sdf->GetElement("xyzOffset")->Get<ignition::math::Vector3d>();
101 
102  if (!_sdf->HasElement("rpyOffset"))
103  {
104  ROS_DEBUG_NAMED("p3d", "p3d plugin missing <rpyOffset>, defaults to 0s");
105  this->offset_.Rot() = ignition::math::Quaterniond(ignition::math::Vector3d(0, 0, 0));
106  }
107  else
108  this->offset_.Rot() = ignition::math::Quaterniond(_sdf->GetElement("rpyOffset")->Get<ignition::math::Vector3d>());
109 
110  if (!_sdf->HasElement("gaussianNoise"))
111  {
112  ROS_DEBUG_NAMED("p3d", "p3d plugin missing <gaussianNoise>, defaults to 0.0");
113  this->gaussian_noise_ = 0;
114  }
115  else
116  this->gaussian_noise_ = _sdf->GetElement("gaussianNoise")->Get<double>();
117 
118  if (!_sdf->HasElement("updateRate"))
119  {
120  ROS_DEBUG_NAMED("p3d", "p3d plugin missing <updateRate>, defaults to 0.0"
121  " (as fast as possible)");
122  this->update_rate_ = 0;
123  }
124  else
125  this->update_rate_ = _sdf->GetElement("updateRate")->Get<double>();
126 
127  // Make sure the ROS node for Gazebo has already been initialized
128  if (!ros::isInitialized())
129  {
130  ROS_FATAL_STREAM_NAMED("p3d", "A ROS node for Gazebo has not been initialized, unable to load plugin. "
131  << "Load the Gazebo system plugin 'libgazebo_ros_api_plugin.so' in the gazebo_ros package)");
132  return;
133  }
134 
135  this->rosnode_ = new ros::NodeHandle(this->robot_namespace_);
136 
137  // publish multi queue
138  this->pmq.startServiceThread();
139 
140  // resolve tf prefix
141  std::string prefix;
142  this->rosnode_->getParam(std::string("tf_prefix"), prefix);
143  this->tf_frame_name_ = tf::resolve(prefix, this->frame_name_);
144 
145  if (this->topic_name_ != "")
146  {
147  this->pub_Queue = this->pmq.addPub<nav_msgs::Odometry>();
148  this->pub_ =
149  this->rosnode_->advertise<nav_msgs::Odometry>(this->topic_name_, 1);
150  }
151 
152 #if GAZEBO_MAJOR_VERSION >= 8
153  this->last_time_ = this->world_->SimTime();
154 #else
155  this->last_time_ = this->world_->GetSimTime();
156 #endif
157  // initialize body
158 #if GAZEBO_MAJOR_VERSION >= 8
159  this->last_vpos_ = this->link_->WorldLinearVel();
160  this->last_veul_ = this->link_->WorldAngularVel();
161 #else
162  this->last_vpos_ = this->link_->GetWorldLinearVel().Ign();
163  this->last_veul_ = this->link_->GetWorldAngularVel().Ign();
164 #endif
165  this->apos_ = 0;
166  this->aeul_ = 0;
167 
168  // if frameName specified is "/world", "world", "/map" or "map" report
169  // back inertial values in the gazebo world
170  if (this->frame_name_ != "/world" &&
171  this->frame_name_ != "world" &&
172  this->frame_name_ != "/map" &&
173  this->frame_name_ != "map")
174  {
175  this->reference_link_ = this->model_->GetLink(this->frame_name_);
176  if (!this->reference_link_)
177  {
178  ROS_ERROR_NAMED("p3d", "gazebo_ros_p3d plugin: frameName: %s does not exist, will"
179  " not publish pose\n", this->frame_name_.c_str());
180  return;
181  }
182  }
183 
184  // init reference frame state
185  if (this->reference_link_)
186  {
187  ROS_DEBUG_NAMED("p3d", "got body %s", this->reference_link_->GetName().c_str());
188  this->frame_apos_ = 0;
189  this->frame_aeul_ = 0;
190 #if GAZEBO_MAJOR_VERSION >= 8
191  this->last_frame_vpos_ = this->reference_link_->WorldLinearVel();
192  this->last_frame_veul_ = this->reference_link_->WorldAngularVel();
193 #else
194  this->last_frame_vpos_ = this->reference_link_->GetWorldLinearVel().Ign();
195  this->last_frame_veul_ = this->reference_link_->GetWorldAngularVel().Ign();
196 #endif
197  }
198 
199 
200  // start custom queue for p3d
201  this->callback_queue_thread_ = boost::thread(
202  boost::bind(&GazeboRosP3D::P3DQueueThread, this));
203 
204  // New Mechanism for Updating every World Cycle
205  // Listen to the update event. This event is broadcast every
206  // simulation iteration.
207  this->update_connection_ = event::Events::ConnectWorldUpdateBegin(
208  boost::bind(&GazeboRosP3D::UpdateChild, this));
209 }
210 
212 // Update the controller
214 {
215  if (!this->link_)
216  return;
217 
218 #if GAZEBO_MAJOR_VERSION >= 8
219  common::Time cur_time = this->world_->SimTime();
220 #else
221  common::Time cur_time = this->world_->GetSimTime();
222 #endif
223 
224  if (cur_time < this->last_time_)
225  {
226  ROS_WARN_NAMED("p3d", "Negative update time difference detected.");
227  this->last_time_ = cur_time;
228  }
229 
230  // rate control
231  if (this->update_rate_ > 0 &&
232  (cur_time-this->last_time_).Double() < (1.0/this->update_rate_))
233  return;
234 
235  if (this->pub_.getNumSubscribers() > 0)
236  {
237  // differentiate to get accelerations
238  double tmp_dt = cur_time.Double() - this->last_time_.Double();
239  if (tmp_dt != 0)
240  {
241  this->lock.lock();
242 
243  if (this->topic_name_ != "")
244  {
245  // copy data into pose message
246  this->pose_msg_.header.frame_id = this->tf_frame_name_;
247  this->pose_msg_.header.stamp.sec = cur_time.sec;
248  this->pose_msg_.header.stamp.nsec = cur_time.nsec;
249 
250  this->pose_msg_.child_frame_id = this->link_name_;
251 
252  ignition::math::Pose3d pose, frame_pose;
253  ignition::math::Vector3d frame_vpos;
254  ignition::math::Vector3d frame_veul;
255 
256  // get inertial Rates
257  // Get Pose/Orientation
258 #if GAZEBO_MAJOR_VERSION >= 8
259  ignition::math::Vector3d vpos = this->link_->WorldLinearVel();
260  ignition::math::Vector3d veul = this->link_->WorldAngularVel();
261 
262  pose = this->link_->WorldPose();
263 #else
264  ignition::math::Vector3d vpos = this->link_->GetWorldLinearVel().Ign();
265  ignition::math::Vector3d veul = this->link_->GetWorldAngularVel().Ign();
266 
267  pose = this->link_->GetWorldPose().Ign();
268 #endif
269 
270  // Apply Reference Frame
271  if (this->reference_link_)
272  {
273  // convert to relative pose, rates
274 #if GAZEBO_MAJOR_VERSION >= 8
275  frame_pose = this->reference_link_->WorldPose();
276  frame_vpos = this->reference_link_->WorldLinearVel();
277  frame_veul = this->reference_link_->WorldAngularVel();
278 #else
279  frame_pose = this->reference_link_->GetWorldPose().Ign();
280  frame_vpos = this->reference_link_->GetWorldLinearVel().Ign();
281  frame_veul = this->reference_link_->GetWorldAngularVel().Ign();
282 #endif
283  pose.Pos() = pose.Pos() - frame_pose.Pos();
284  pose.Pos() = frame_pose.Rot().RotateVectorReverse(pose.Pos());
285  pose.Rot() *= frame_pose.Rot().Inverse();
286 
287  vpos = frame_pose.Rot().RotateVector(vpos - frame_vpos);
288  veul = frame_pose.Rot().RotateVector(veul - frame_veul);
289  }
290 
291  // Apply Constant Offsets
292  // apply xyz offsets and get position and rotation components
293  pose.Pos() = pose.Pos() + this->offset_.Pos();
294  // apply rpy offsets
295  pose.Rot() = this->offset_.Rot()*pose.Rot();
296  pose.Rot().Normalize();
297 
298  // compute accelerations (not used)
299  this->apos_ = (this->last_vpos_ - vpos) / tmp_dt;
300  this->aeul_ = (this->last_veul_ - veul) / tmp_dt;
301  this->last_vpos_ = vpos;
302  this->last_veul_ = veul;
303 
304  this->frame_apos_ = (this->last_frame_vpos_ - frame_vpos) / tmp_dt;
305  this->frame_aeul_ = (this->last_frame_veul_ - frame_veul) / tmp_dt;
306  this->last_frame_vpos_ = frame_vpos;
307  this->last_frame_veul_ = frame_veul;
308 
309  // Fill out messages
310  this->pose_msg_.pose.pose.position.x = pose.Pos().X();
311  this->pose_msg_.pose.pose.position.y = pose.Pos().Y();
312  this->pose_msg_.pose.pose.position.z = pose.Pos().Z();
313 
314  this->pose_msg_.pose.pose.orientation.x = pose.Rot().X();
315  this->pose_msg_.pose.pose.orientation.y = pose.Rot().Y();
316  this->pose_msg_.pose.pose.orientation.z = pose.Rot().Z();
317  this->pose_msg_.pose.pose.orientation.w = pose.Rot().W();
318 
319  this->pose_msg_.twist.twist.linear.x = vpos.X() +
320  this->GaussianKernel(0, this->gaussian_noise_);
321  this->pose_msg_.twist.twist.linear.y = vpos.Y() +
322  this->GaussianKernel(0, this->gaussian_noise_);
323  this->pose_msg_.twist.twist.linear.z = vpos.Z() +
324  this->GaussianKernel(0, this->gaussian_noise_);
325  // pass euler angular rates
326  this->pose_msg_.twist.twist.angular.x = veul.X() +
327  this->GaussianKernel(0, this->gaussian_noise_);
328  this->pose_msg_.twist.twist.angular.y = veul.Y() +
329  this->GaussianKernel(0, this->gaussian_noise_);
330  this->pose_msg_.twist.twist.angular.z = veul.Z() +
331  this->GaussianKernel(0, this->gaussian_noise_);
332 
333  // fill in covariance matrix
335  double gn2 = this->gaussian_noise_*this->gaussian_noise_;
336  this->pose_msg_.pose.covariance[0] = gn2;
337  this->pose_msg_.pose.covariance[7] = gn2;
338  this->pose_msg_.pose.covariance[14] = gn2;
339  this->pose_msg_.pose.covariance[21] = gn2;
340  this->pose_msg_.pose.covariance[28] = gn2;
341  this->pose_msg_.pose.covariance[35] = gn2;
342 
343  this->pose_msg_.twist.covariance[0] = gn2;
344  this->pose_msg_.twist.covariance[7] = gn2;
345  this->pose_msg_.twist.covariance[14] = gn2;
346  this->pose_msg_.twist.covariance[21] = gn2;
347  this->pose_msg_.twist.covariance[28] = gn2;
348  this->pose_msg_.twist.covariance[35] = gn2;
349 
350  // publish to ros
351  this->pub_Queue->push(this->pose_msg_, this->pub_);
352  }
353 
354  this->lock.unlock();
355 
356  // save last time stamp
357  this->last_time_ = cur_time;
358  }
359  }
360 }
361 
363 // Utility for adding noise
364 double GazeboRosP3D::GaussianKernel(double mu, double sigma)
365 {
366  // using Box-Muller transform to generate two independent standard
367  // normally disbributed normal variables see wikipedia
368 
369  // normalized uniform random variable
370  double U = static_cast<double>(rand_r(&this->seed)) /
371  static_cast<double>(RAND_MAX);
372 
373  // normalized uniform random variable
374  double V = static_cast<double>(rand_r(&this->seed)) /
375  static_cast<double>(RAND_MAX);
376 
377  double X = sqrt(-2.0 * ::log(U)) * cos(2.0*M_PI * V);
378  // double Y = sqrt(-2.0 * ::log(U)) * sin(2.0*M_PI * V);
379 
380  // there are 2 indep. vars, we'll just use X
381  // scale to our mu and sigma
382  X = sigma * X + mu;
383  return X;
384 }
385 
387 // Put laser data to the interface
389 {
390  static const double timeout = 0.01;
391 
392  while (this->rosnode_->ok())
393  {
395  }
396 }
397 }
ignition::math::Pose3d offset_
allow specifying constant xyz and rpy offsets
void push(T &msg, ros::Publisher &pub)
Push a new message onto the queue.
Definition: PubQueue.h:73
double gaussian_noise_
Gaussian noise.
double GaussianKernel(double mu, double sigma)
Gaussian noise generator.
GazeboRosP3D()
Constructor.
ignition::math::Vector3d frame_aeul_
physics::LinkPtr reference_link_
The body of the frame to display pose, twist.
std::string frame_name_
frame transform name, should match link name FIXME: extract link name directly?
#define ROS_WARN_NAMED(name,...)
event::ConnectionPtr update_connection_
virtual void UpdateChild()
Update the controller.
ROSCPP_DECL bool isInitialized()
boost::mutex lock
mutex to lock access to fields used in message callbacks
ros::NodeHandle * rosnode_
pointer to ros node
virtual ~GazeboRosP3D()
Destructor.
std::string robot_namespace_
for setting ROS name space
ignition::math::Vector3d aeul_
physics::ModelPtr model_
ignition::math::Vector3d last_frame_vpos_
std::string resolve(const std::string &prefix, const std::string &frame_name)
#define ROS_DEBUG_NAMED(name,...)
GZ_REGISTER_MODEL_PLUGIN(GazeboRosF3D)
ignition::math::Vector3d last_vpos_
physics::WorldPtr world_
PubQueue< nav_msgs::Odometry >::Ptr pub_Queue
ros::CallbackQueue p3d_queue_
ignition::math::Vector3d apos_
#define ROS_FATAL_STREAM_NAMED(name, args)
ignition::math::Vector3d last_veul_
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
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
common::Time last_time_
save last_time
void Load(physics::ModelPtr _parent, sdf::ElementPtr _sdf)
Load the controller.
physics::LinkPtr link_
The parent Model.
uint32_t getNumSubscribers() const
bool getParam(const std::string &key, std::string &s) const
#define ROS_FATAL_NAMED(name,...)
#define ROS_ERROR_NAMED(name,...)
void startServiceThread()
Start a thread to call spin().
Definition: PubQueue.h:181
ignition::math::Vector3d frame_apos_
boost::thread callback_queue_thread_
ignition::math::Vector3d last_frame_veul_
std::string topic_name_
topic name
bool ok() const
nav_msgs::Odometry pose_msg_
ros message
std::string tf_frame_name_
ros::Publisher pub_
std::string link_name_
store bodyname


gazebo_plugins
Author(s): John Hsu
autogenerated on Tue Apr 6 2021 02:19:39