24 #include <ignition/math/Rand.hh> 53 this->
world_ = _parent->GetWorld();
67 if (this->
sdf->HasElement(
"robotNamespace"))
70 if (!this->
sdf->HasElement(
"serviceName"))
72 ROS_INFO_NAMED(
"imu",
"imu plugin missing <serviceName>, defaults to /default_imu");
78 if (!this->
sdf->HasElement(
"topicName"))
80 ROS_INFO_NAMED(
"imu",
"imu plugin missing <topicName>, defaults to /default_imu");
86 if (!this->
sdf->HasElement(
"gaussianNoise"))
88 ROS_INFO_NAMED(
"imu",
"imu plugin missing <gaussianNoise>, defaults to 0.0");
94 if (!this->
sdf->HasElement(
"bodyName"))
96 ROS_FATAL_NAMED(
"imu",
"imu plugin missing <bodyName>, cannot proceed");
102 if (!this->
sdf->HasElement(
"xyzOffset"))
104 ROS_INFO_NAMED(
"imu",
"imu plugin missing <xyzOffset>, defaults to 0s");
105 this->
offset_.Pos() = ignition::math::Vector3d(0, 0, 0);
108 this->
offset_.Pos() = this->
sdf->Get<ignition::math::Vector3d>(
"xyzOffset");
110 if (!this->
sdf->HasElement(
"rpyOffset"))
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));
116 this->
offset_.Rot() = ignition::math::Quaterniond(this->
sdf->Get<ignition::math::Vector3d>(
"rpyOffset"));
118 if (!this->
sdf->HasElement(
"updateRate"))
120 ROS_DEBUG_NAMED(
"imu",
"imu plugin missing <updateRate>, defaults to 0.0" 121 " (as fast as possible)");
125 this->
update_rate_ = this->
sdf->GetElement(
"updateRate")->Get<
double>();
127 if (!this->
sdf->HasElement(
"frameName"))
129 ROS_INFO_NAMED(
"imu",
"imu plugin missing <frameName>, defaults to <bodyName>");
139 <<
"Load the Gazebo system plugin 'libgazebo_ros_api_plugin.so' in the gazebo_ros package)");
149 this->
link = boost::dynamic_pointer_cast<physics::Link>(
150 #if GAZEBO_MAJOR_VERSION >= 8 157 ROS_FATAL_NAMED(
"imu",
"gazebo_ros_imu plugin error: bodyName: %s does not exist\n",
171 ros::AdvertiseServiceOptions::create<std_srvs::Empty>(
178 #if GAZEBO_MAJOR_VERSION >= 8 185 #if GAZEBO_MAJOR_VERSION >= 8 210 std_srvs::Empty::Response &res)
219 #if GAZEBO_MAJOR_VERSION >= 8 220 common::Time cur_time = this->
world_->SimTime();
222 common::Time cur_time = this->
world_->GetSimTime();
232 ignition::math::Pose3d pose;
233 ignition::math::Quaterniond rot;
234 ignition::math::Vector3d pos;
237 #if GAZEBO_MAJOR_VERSION >= 8 238 pose = this->
link->WorldPose();
240 pose = this->
link->GetWorldPose().Ign();
243 pos = pose.Pos() + this->
offset_.Pos();
251 #if GAZEBO_MAJOR_VERSION >= 8 252 ignition::math::Vector3d vpos = this->
link->WorldLinearVel();
253 ignition::math::Vector3d veul = this->
link->WorldAngularVel();
255 ignition::math::Vector3d vpos = this->
link->GetWorldLinearVel().Ign();
256 ignition::math::Vector3d veul = this->
link->GetWorldAngularVel().Ign();
260 double tmp_dt = this->
last_time_.Double() - cur_time.Double();
271 this->
imu_msg_.header.stamp.sec = cur_time.sec;
272 this->
imu_msg_.header.stamp.nsec = cur_time.nsec;
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();
288 ignition::math::Vector3d linear_velocity(
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();
300 ignition::math::Vector3d linear_acceleration(
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();
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;
326 boost::mutex::scoped_lock lock(this->
lock_);
346 double U = ignition::math::Rand::DblUniform();
349 double V = ignition::math::Rand::DblUniform();
351 double X = sqrt(-2.0 * ::log(U)) * cos(2.0*M_PI * V);
364 static const double timeout = 0.01;
sensor_msgs::Imu imu_msg_
ros message
void push(T &msg, ros::Publisher &pub)
Push a new message onto the queue.
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,...)
common::Time last_time_
save last_time
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)...
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().
double GaussianKernel(double mu, double sigma)
Gaussian noise generator.
uint32_t getNumSubscribers() const
boost::shared_ptr< void > VoidPtr
bool ServiceCallback(std_srvs::Empty::Request &req, std_srvs::Empty::Response &res)
call back when using service