gazebo_ros_imu_sensor.cpp
Go to the documentation of this file.
1 /* Copyright [2015] [Alessandro Settimi]
2  *
3  * email: ale.settimi@gmail.com
4  *
5  * Licensed under the Apache License, Version 2.0 (the "License");
6  * you may not use this file except in compliance with the License.
7  * You may obtain a copy of the License at
8  *
9  * http://www.apache.org/licenses/LICENSE-2.0
10  *
11  * Unless required by applicable law or agreed to in writing, software
12  * distributed under the License is distributed on an "AS IS" BASIS,
13  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
14  * See the License for the specific language governing permissions and
15  * limitations under the License.*/
16 
18 #include <iostream>
19 #include <gazebo/sensors/ImuSensor.hh>
20 #include <gazebo/physics/World.hh>
21 
22 GZ_REGISTER_SENSOR_PLUGIN(gazebo::GazeboRosImuSensor)
23 
25 {
26  accelerometer_data = ignition::math::Vector3d(0, 0, 0);
27  gyroscope_data = ignition::math::Vector3d(0, 0, 0);
28  orientation = ignition::math::Quaterniond(1,0,0,0);
29  seed=0;
30  sensor=NULL;
31 }
32 
33 void gazebo::GazeboRosImuSensor::Load(gazebo::sensors::SensorPtr sensor_, sdf::ElementPtr sdf_)
34 {
35  sdf=sdf_;
36  sensor=dynamic_cast<gazebo::sensors::ImuSensor*>(sensor_.get());
37 
38  if(sensor==NULL)
39  {
40  ROS_FATAL("Error: Sensor pointer is NULL!");
41  return;
42  }
43 
44  sensor->SetActive(true);
45 
46  if(!LoadParameters())
47  {
48  ROS_FATAL("Error Loading Parameters!");
49  return;
50  }
51 
52  if (!ros::isInitialized()) //check if ros is initialized properly
53  {
54  ROS_FATAL("ROS has not been initialized!");
55  return;
56  }
57 
59 
60  imu_data_publisher = node->advertise<sensor_msgs::Imu>(topic_name,1);
61 
62  connection = gazebo::event::Events::ConnectWorldUpdateBegin(boost::bind(&GazeboRosImuSensor::UpdateChild, this, _1));
63 
64  last_time = sensor->LastUpdateTime();
65 }
66 
67 void gazebo::GazeboRosImuSensor::UpdateChild(const gazebo::common::UpdateInfo &/*_info*/)
68 {
69  common::Time current_time = sensor->LastUpdateTime();
70 
71  if(update_rate>0 && (current_time-last_time).Double() < 1.0/update_rate) //update rate check
72  return;
73 
75  {
76  orientation = offset.Rot()*sensor->Orientation(); //applying offsets to the orientation measurement
77  accelerometer_data = sensor->LinearAcceleration();
78  gyroscope_data = sensor->AngularVelocity();
79 
80  //Guassian noise is applied to all measurements
81  imu_msg.orientation.x = orientation.X() + GuassianKernel(0,gaussian_noise);
82  imu_msg.orientation.y = orientation.Y() + GuassianKernel(0,gaussian_noise);
83  imu_msg.orientation.z = orientation.Z() + GuassianKernel(0,gaussian_noise);
84  imu_msg.orientation.w = orientation.W() + GuassianKernel(0,gaussian_noise);
85 
86  imu_msg.linear_acceleration.x = accelerometer_data.X() + GuassianKernel(0,gaussian_noise);
87  imu_msg.linear_acceleration.y = accelerometer_data.Y() + GuassianKernel(0,gaussian_noise);
88  imu_msg.linear_acceleration.z = accelerometer_data.Z() + GuassianKernel(0,gaussian_noise);
89 
90  imu_msg.angular_velocity.x = gyroscope_data.X() + GuassianKernel(0,gaussian_noise);
91  imu_msg.angular_velocity.y = gyroscope_data.Y() + GuassianKernel(0,gaussian_noise);
92  imu_msg.angular_velocity.z = gyroscope_data.Z() + GuassianKernel(0,gaussian_noise);
93 
94  //covariance is related to the Gaussian noise
95  double gn2 = gaussian_noise*gaussian_noise;
96  imu_msg.orientation_covariance[0] = gn2;
97  imu_msg.orientation_covariance[4] = gn2;
98  imu_msg.orientation_covariance[8] = gn2;
99  imu_msg.angular_velocity_covariance[0] = gn2;
100  imu_msg.angular_velocity_covariance[4] = gn2;
101  imu_msg.angular_velocity_covariance[8] = gn2;
102  imu_msg.linear_acceleration_covariance[0] = gn2;
103  imu_msg.linear_acceleration_covariance[4] = gn2;
104  imu_msg.linear_acceleration_covariance[8] = gn2;
105 
106  //preparing message header
107  imu_msg.header.frame_id = body_name;
108  imu_msg.header.stamp.sec = current_time.sec;
109  imu_msg.header.stamp.nsec = current_time.nsec;
110 
111  //publishing data
113 
114  ros::spinOnce();
115  }
116 
117  last_time = current_time;
118 }
119 
120 double gazebo::GazeboRosImuSensor::GuassianKernel(double mu, double sigma)
121 {
122  // generation of two normalized uniform random variables
123  double U1 = static_cast<double>(rand_r(&seed)) / static_cast<double>(RAND_MAX);
124  double U2 = static_cast<double>(rand_r(&seed)) / static_cast<double>(RAND_MAX);
125 
126  // using Box-Muller transform to obtain a varaible with a standard normal distribution
127  double Z0 = sqrt(-2.0 * ::log(U1)) * cos(2.0*M_PI * U2);
128 
129  // scaling
130  Z0 = sigma * Z0 + mu;
131  return Z0;
132 }
133 
135 {
136  //loading parameters from the sdf file
137 
138  //NAMESPACE
139  if (sdf->HasElement("robotNamespace"))
140  {
141  robot_namespace = sdf->Get<std::string>("robotNamespace") +"/";
142  ROS_INFO_STREAM("<robotNamespace> set to: "<<robot_namespace);
143  }
144  else
145  {
146  std::string scoped_name = sensor->ParentName();
147  std::size_t it = scoped_name.find("::");
148 
149  robot_namespace = "/" +scoped_name.substr(0,it)+"/";
150  ROS_WARN_STREAM("missing <robotNamespace>, set to default: " << robot_namespace);
151  }
152 
153  //TOPIC
154  if (sdf->HasElement("topicName"))
155  {
156  topic_name = robot_namespace + sdf->Get<std::string>("topicName");
157  ROS_INFO_STREAM("<topicName> set to: "<<topic_name);
158  }
159  else
160  {
161  topic_name = robot_namespace + "/imu_data";
162  ROS_WARN_STREAM("missing <topicName>, set to /namespace/default: " << topic_name);
163  }
164 
165  //BODY NAME
166  if (sdf->HasElement("frameName"))
167  {
168  body_name = sdf->Get<std::string>("frameName");
169  ROS_INFO_STREAM("<frameName> set to: "<<body_name);
170  }
171  else
172  {
173  ROS_FATAL("missing <frameName>, cannot proceed");
174  return false;
175  }
176 
177  //UPDATE RATE
178  if (sdf->HasElement("updateRateHZ"))
179  {
180  update_rate = sdf->Get<double>("updateRateHZ");
181  ROS_INFO_STREAM("<updateRateHZ> set to: " << update_rate);
182  }
183  else
184  {
185  update_rate = 1.0;
186  ROS_WARN_STREAM("missing <updateRateHZ>, set to default: " << update_rate);
187  }
188 
189  //NOISE
190  if (sdf->HasElement("gaussianNoise"))
191  {
192  gaussian_noise = sdf->Get<double>("gaussianNoise");
193  ROS_INFO_STREAM("<gaussianNoise> set to: " << gaussian_noise);
194  }
195  else
196  {
197  gaussian_noise = 0.0;
198  ROS_WARN_STREAM("missing <gaussianNoise>, set to default: " << gaussian_noise);
199  }
200 
201  //POSITION OFFSET, UNUSED
202  if (sdf->HasElement("xyzOffset"))
203  {
204  offset.Pos() = sdf->Get<ignition::math::Vector3d>("xyzOffset");
205  ROS_INFO_STREAM("<xyzOffset> set to: " << offset.Pos()[0] << ' ' << offset.Pos()[1] << ' ' << offset.Pos()[2]);
206  }
207  else
208  {
209  offset.Pos() = ignition::math::Vector3d(0, 0, 0);
210  ROS_WARN_STREAM("missing <xyzOffset>, set to default: " << offset.Pos()[0] << ' ' << offset.Pos()[1] << ' ' << offset.Pos()[2]);
211  }
212 
213  //ORIENTATION OFFSET
214  if (sdf->HasElement("rpyOffset"))
215  {
216  offset.Rot() = ignition::math::Quaterniond(sdf->Get<ignition::math::Vector3d>("rpyOffset"));
217  ROS_INFO_STREAM("<rpyOffset> set to: " << offset.Rot().Roll() << ' ' << offset.Rot().Pitch() << ' ' << offset.Rot().Yaw());
218  }
219  else
220  {
221  offset.Rot() = ignition::math::Quaterniond::Identity;
222  ROS_WARN_STREAM("missing <rpyOffset>, set to default: " << offset.Rot().Roll() << ' ' << offset.Rot().Pitch() << ' ' << offset.Rot().Yaw());
223  }
224 
225  return true;
226 }
227 
229 {
230  if (connection.get())
231  {
232  connection.reset();
233  connection = gazebo::event::ConnectionPtr();
234  }
235 
236  node->shutdown();
237 }
double update_rate
Sensor update rate.
sdf::ElementPtr sdf
Pointer to the sdf config file.
#define ROS_FATAL(...)
void publish(const boost::shared_ptr< M > &message) const
std::string robot_namespace
The data is published on the topic named: /robot_namespace/topic_name.
ROSCPP_DECL bool isInitialized()
double GuassianKernel(double mu, double sigma)
Gaussian noise generator.
ros::Publisher imu_data_publisher
Ros Publisher for imu data.
Gazebo Ros imu sensor plugin.
double gaussian_noise
Gaussian noise.
ignition::math::Vector3d accelerometer_data
Linear acceleration data from the sensor.
ignition::math::Vector3d gyroscope_data
Angular velocity data from the sensor.
ros::NodeHandle * node
Ros NodeHandle pointer.
sensors::ImuSensor * sensor
Pointer to the sensor.
virtual void Load(sensors::SensorPtr sensor_, sdf::ElementPtr sdf_)
Load the sensor.
unsigned int seed
Seed for the Gaussian noise generator.
ignition::math::Quaterniond orientation
Orientation data from the sensor.
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
virtual void UpdateChild(const gazebo::common::UpdateInfo &)
Update the sensor.
#define ROS_WARN_STREAM(args)
virtual ~GazeboRosImuSensor()
Destructor.
bool LoadParameters()
Load the parameters from the sdf file.
common::Time last_time
last time on which the data was published.
sensor_msgs::Imu imu_msg
Ros IMU message.
uint32_t getNumSubscribers() const
#define ROS_INFO_STREAM(args)
gazebo::event::ConnectionPtr connection
Pointer to the update event connection.
std::string topic_name
The data is published on the topic named: /robot_namespace/topic_name.
ROSCPP_DECL void spinOnce()
ignition::math::Pose3d offset
Offset parameter, position part is unused.
std::string body_name
Name of the link of the IMU.


gazebo_plugins
Author(s): John Hsu
autogenerated on Tue Mar 26 2019 02:31:27