gazebo_ros_imu.cpp
Go to the documentation of this file.
00001 //=================================================================================================
00002 // Copyright (c) 2012, Johannes Meyer, TU Darmstadt
00003 // All rights reserved.
00004 
00005 // Redistribution and use in source and binary forms, with or without
00006 // modification, are permitted provided that the following conditions are met:
00007 //     * Redistributions of source code must retain the above copyright
00008 //       notice, this list of conditions and the following disclaimer.
00009 //     * Redistributions in binary form must reproduce the above copyright
00010 //       notice, this list of conditions and the following disclaimer in the
00011 //       documentation and/or other materials provided with the distribution.
00012 //     * Neither the name of the Flight Systems and Automatic Control group,
00013 //       TU Darmstadt, nor the names of its contributors may be used to
00014 //       endorse or promote products derived from this software without
00015 //       specific prior written permission.
00016 
00017 // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
00018 // ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
00019 // WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
00020 // DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER BE LIABLE FOR ANY
00021 // DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
00022 // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00023 // LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
00024 // ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
00025 // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
00026 // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
00027 //=================================================================================================
00028 
00029 #include <hector_gazebo_plugins/gazebo_ros_imu.h>
00030 #include <gazebo/common/Events.hh>
00031 #include <gazebo/physics/physics.hh>
00032 
00033 #include <ros/console.h>
00034 
00035 namespace gazebo
00036 {
00037 
00038 // #define DEBUG_OUTPUT
00039 #ifdef DEBUG_OUTPUT
00040   #include <geometry_msgs/PoseStamped.h>
00041   static ros::Publisher debugPublisher;
00042 #endif // DEBUG_OUTPUT
00043 
00045 // Constructor
00046 GazeboRosIMU::GazeboRosIMU()
00047 {
00048 }
00049 
00051 // Destructor
00052 GazeboRosIMU::~GazeboRosIMU()
00053 {
00054   updateTimer.Disconnect(updateConnection);
00055 
00056   dynamic_reconfigure_server_accel_.reset();
00057   dynamic_reconfigure_server_rate_.reset();
00058   dynamic_reconfigure_server_yaw_.reset();
00059 
00060   node_handle_->shutdown();
00061 #ifdef USE_CBQ
00062   callback_queue_thread_.join();
00063 #endif
00064   delete node_handle_;
00065 }
00066 
00068 // Load the controller
00069 void GazeboRosIMU::Load(physics::ModelPtr _model, sdf::ElementPtr _sdf)
00070 {
00071   // Get the world name.
00072   world = _model->GetWorld();
00073 
00074   // load parameters
00075   if (_sdf->HasElement("robotNamespace"))
00076     namespace_ = _sdf->GetElement("robotNamespace")->GetValue()->GetAsString();
00077   else
00078     namespace_.clear();
00079 
00080   if (_sdf->HasElement("bodyName"))
00081   {
00082     link_name_ = _sdf->GetElement("bodyName")->GetValue()->GetAsString();
00083     link = _model->GetLink(link_name_);
00084   }
00085   else
00086   {
00087     link = _model->GetLink();
00088     link_name_ = link->GetName();
00089   }
00090 
00091   // assert that the body by link_name_ exists
00092   if (!link)
00093   {
00094     ROS_FATAL("GazeboRosIMU plugin error: bodyName: %s does not exist\n", link_name_.c_str());
00095     return;
00096   }
00097 
00098   // default parameters
00099   frame_id_ = link_name_;
00100   topic_ = "imu";
00101 
00102   if (_sdf->HasElement("frameId"))
00103     frame_id_ = _sdf->GetElement("frameId")->GetValue()->GetAsString();
00104 
00105   if (_sdf->HasElement("topicName"))
00106     topic_ = _sdf->GetElement("topicName")->GetValue()->GetAsString();
00107 
00108   if (_sdf->HasElement("biasTopicName"))
00109     bias_topic_ = _sdf->GetElement("biasTopicName")->GetValue()->GetAsString();
00110   else
00111     bias_topic_ = (!topic_.empty() ? topic_ + "/bias" : "");
00112 
00113   if (_sdf->HasElement("serviceName"))
00114     serviceName = _sdf->GetElement("serviceName")->GetValue()->GetAsString();
00115   else
00116     serviceName = topic_ + "/calibrate";
00117 
00118   accelModel.Load(_sdf, "accel");
00119   rateModel.Load(_sdf, "rate");
00120   yawModel.Load(_sdf, "yaw");
00121 
00122   // also use old configuration variables from gazebo_ros_imu
00123   if (_sdf->HasElement("gaussianNoise")) {
00124     double gaussianNoise;
00125     if (_sdf->GetElement("gaussianNoise")->GetValue()->Get(gaussianNoise) && gaussianNoise != 0.0) {
00126       accelModel.gaussian_noise = gaussianNoise;
00127       rateModel.gaussian_noise  = gaussianNoise;
00128     }
00129   }
00130 
00131   if (_sdf->HasElement("xyzOffset")) {
00132     this->offset_.pos = _sdf->Get<math::Vector3>("xyzOffset");
00133   } else {
00134     ROS_INFO("imu plugin missing <xyzOffset>, defaults to 0s");
00135     this->offset_.pos = math::Vector3(0, 0, 0);
00136   }
00137 
00138   if (_sdf->HasElement("rpyOffset")) {
00139     this->offset_.rot = _sdf->Get<math::Vector3>("rpyOffset");
00140   } else {
00141     ROS_INFO("imu plugin missing <rpyOffset>, defaults to 0s");
00142     this->offset_.rot = math::Vector3(0, 0, 0);
00143   }
00144 
00145   // fill in constant covariance matrix
00146   imuMsg.angular_velocity_covariance[0] = rateModel.gaussian_noise.x*rateModel.gaussian_noise.x;
00147   imuMsg.angular_velocity_covariance[4] = rateModel.gaussian_noise.y*rateModel.gaussian_noise.y;
00148   imuMsg.angular_velocity_covariance[8] = rateModel.gaussian_noise.z*rateModel.gaussian_noise.z;
00149   imuMsg.linear_acceleration_covariance[0] = accelModel.gaussian_noise.x*accelModel.gaussian_noise.x;
00150   imuMsg.linear_acceleration_covariance[4] = accelModel.gaussian_noise.y*accelModel.gaussian_noise.y;
00151   imuMsg.linear_acceleration_covariance[8] = accelModel.gaussian_noise.z*accelModel.gaussian_noise.z;
00152 
00153   // Make sure the ROS node for Gazebo has already been initialized
00154   if (!ros::isInitialized())
00155   {
00156     ROS_FATAL_STREAM("A ROS node for Gazebo has not been initialized, unable to load plugin. "
00157       << "Load the Gazebo system plugin 'libgazebo_ros_api_plugin.so' in the gazebo_ros package.");
00158     return;
00159   }
00160 
00161   node_handle_ = new ros::NodeHandle(namespace_);
00162 
00163   // if topic name specified as empty, do not publish (then what is this plugin good for?)
00164   if (!topic_.empty())
00165     pub_ = node_handle_->advertise<sensor_msgs::Imu>(topic_, 10);
00166   if (!bias_topic_.empty())
00167     bias_pub_ = node_handle_->advertise<sensor_msgs::Imu>(bias_topic_, 10);
00168 
00169 #ifdef DEBUG_OUTPUT
00170   debugPublisher = rosnode_->advertise<geometry_msgs::PoseStamped>(topic_ + "/pose", 10);
00171 #endif // DEBUG_OUTPUT
00172 
00173   // advertise services for calibration and bias setting
00174   if (!serviceName.empty())
00175     srv_ = node_handle_->advertiseService(serviceName, &GazeboRosIMU::ServiceCallback, this);
00176 
00177   accelBiasService = node_handle_->advertiseService(topic_ + "/set_accel_bias", &GazeboRosIMU::SetAccelBiasCallback, this);
00178   rateBiasService  = node_handle_->advertiseService(topic_ + "/set_rate_bias", &GazeboRosIMU::SetRateBiasCallback, this);
00179 
00180   // setup dynamic_reconfigure servers
00181   if (!topic_.empty()) {
00182     dynamic_reconfigure_server_accel_.reset(new dynamic_reconfigure::Server<SensorModelConfig>(ros::NodeHandle(*node_handle_, topic_ + "/accel")));
00183     dynamic_reconfigure_server_rate_.reset(new dynamic_reconfigure::Server<SensorModelConfig>(ros::NodeHandle(*node_handle_, topic_ + "/rate")));
00184     dynamic_reconfigure_server_yaw_.reset(new dynamic_reconfigure::Server<SensorModelConfig>(ros::NodeHandle(*node_handle_, topic_ + "/yaw")));
00185     dynamic_reconfigure_server_accel_->setCallback(boost::bind(&SensorModel3::dynamicReconfigureCallback, &accelModel, _1, _2));
00186     dynamic_reconfigure_server_rate_->setCallback(boost::bind(&SensorModel3::dynamicReconfigureCallback, &rateModel, _1, _2));
00187     dynamic_reconfigure_server_yaw_->setCallback(boost::bind(&SensorModel::dynamicReconfigureCallback, &yawModel, _1, _2));
00188   }
00189 
00190 #ifdef USE_CBQ
00191   // start custom queue for imu
00192   callback_queue_thread_ = boost::thread( boost::bind( &GazeboRosIMU::CallbackQueueThread,this ) );
00193 #endif
00194 
00195   Reset();
00196 
00197   // connect Update function
00198   updateTimer.Load(world, _sdf);
00199   updateConnection = updateTimer.Connect(boost::bind(&GazeboRosIMU::Update, this));
00200 }
00201 
00202 void GazeboRosIMU::Reset()
00203 {
00204   updateTimer.Reset();
00205 
00206   orientation = math::Quaternion();
00207   velocity = 0.0;
00208   accel = 0.0;
00209 
00210   accelModel.reset();
00211   rateModel.reset();
00212   yawModel.reset();
00213 }
00214 
00216 // returns true always, imu is always calibrated in sim
00217 bool GazeboRosIMU::ServiceCallback(std_srvs::Empty::Request &req,
00218                                         std_srvs::Empty::Response &res)
00219 {
00220   boost::mutex::scoped_lock scoped_lock(lock);
00221   rateModel.reset(math::Vector3(0.0, 0.0, 0.0));
00222   return true;
00223 }
00224 
00225 bool GazeboRosIMU::SetAccelBiasCallback(hector_gazebo_plugins::SetBias::Request &req, hector_gazebo_plugins::SetBias::Response &res)
00226 {
00227   boost::mutex::scoped_lock scoped_lock(lock);
00228   accelModel.reset(math::Vector3(req.bias.x, req.bias.y, req.bias.z));
00229   return true;
00230 }
00231 
00232 bool GazeboRosIMU::SetRateBiasCallback(hector_gazebo_plugins::SetBias::Request &req, hector_gazebo_plugins::SetBias::Response &res)
00233 {
00234   boost::mutex::scoped_lock scoped_lock(lock);
00235   rateModel.reset(math::Vector3(req.bias.x, req.bias.y, req.bias.z));
00236   return true;
00237 }
00238 
00240 // Update the controller
00241 void GazeboRosIMU::Update()
00242 {
00243   // Get Time Difference dt
00244   common::Time cur_time = world->GetSimTime();
00245   double dt = updateTimer.getTimeSinceLastUpdate().Double();
00246   boost::mutex::scoped_lock scoped_lock(lock);
00247 
00248   // Get Pose/Orientation
00249   math::Pose pose = link->GetWorldPose();
00250   // math::Vector3 pos = pose.pos + this->offset_.pos;
00251   math::Quaternion rot = this->offset_.rot * pose.rot;
00252   rot.Normalize();
00253 
00254   // get Gravity
00255   gravity = world->GetPhysicsEngine()->GetGravity();
00256   double gravity_length = gravity.GetLength();
00257   ROS_DEBUG_NAMED("gazebo_ros_imu", "gravity_world = [%g %g %g]", gravity.x, gravity.y, gravity.z);
00258 
00259   // get Acceleration and Angular Rates
00260   // the result of GetRelativeLinearAccel() seems to be unreliable (sum of forces added during the current simulation step)?
00261   //accel = myBody->GetRelativeLinearAccel(); // get acceleration in body frame
00262   math::Vector3 temp = link->GetWorldLinearVel(); // get velocity in world frame
00263   if (dt > 0.0) accel = rot.RotateVectorReverse((temp - velocity) / dt - gravity);
00264   velocity = temp;
00265 
00266   // calculate angular velocity from delta quaternion
00267   // note: link->GetRelativeAngularVel() sometimes return nan?
00268   // rate  = link->GetRelativeAngularVel(); // get angular rate in body frame
00269   math::Quaternion delta = this->orientation.GetInverse() * rot;
00270   this->orientation = rot;
00271   if (dt > 0.0) {
00272     rate = this->offset_.rot.GetInverse()
00273            * (2.0 * acos(std::max(std::min(delta.w, 1.0), -1.0)) * math::Vector3(delta.x, delta.y, delta.z).Normalize() / dt);
00274   }
00275 
00276   // update sensor models
00277   accel = accelModel(accel, dt);
00278   rate  = rateModel(rate, dt);
00279   yawModel.update(dt);
00280   ROS_DEBUG_NAMED("gazebo_ros_imu", "Current bias errors: accel = [%g %g %g], rate = [%g %g %g], yaw = %g",
00281                  accelModel.getCurrentBias().x, accelModel.getCurrentBias().y, accelModel.getCurrentBias().z,
00282                  rateModel.getCurrentBias().x, rateModel.getCurrentBias().y, rateModel.getCurrentBias().z,
00283                  yawModel.getCurrentBias());
00284   ROS_DEBUG_NAMED("gazebo_ros_imu", "Scale errors: accel = [%g %g %g], rate = [%g %g %g], yaw = %g",
00285                  accelModel.getScaleError().x, accelModel.getScaleError().y, accelModel.getScaleError().z,
00286                  rateModel.getScaleError().x, rateModel.getScaleError().y, rateModel.getScaleError().z,
00287                  yawModel.getScaleError());
00288 
00289 
00290   // apply accelerometer and yaw drift error to orientation (pseudo AHRS)
00291   math::Vector3 accelDrift = pose.rot.RotateVector(accelModel.getCurrentBias());
00292   double yawError = yawModel.getCurrentBias();
00293   math::Quaternion orientationError(
00294     math::Quaternion(cos(yawError/2), 0.0, 0.0, sin(yawError/2)) *                                         // yaw error
00295     math::Quaternion(1.0, 0.5 * accelDrift.y / gravity_length, 0.5 * -accelDrift.x / gravity_length, 0.0)  // roll and pitch error
00296   );
00297   orientationError.Normalize();
00298   rot = orientationError * rot;
00299 
00300   // copy data into pose message
00301   imuMsg.header.frame_id = frame_id_;
00302   imuMsg.header.stamp.sec = cur_time.sec;
00303   imuMsg.header.stamp.nsec = cur_time.nsec;
00304 
00305   // orientation quaternion
00306   imuMsg.orientation.x = rot.x;
00307   imuMsg.orientation.y = rot.y;
00308   imuMsg.orientation.z = rot.z;
00309   imuMsg.orientation.w = rot.w;
00310 
00311   // pass angular rates
00312   imuMsg.angular_velocity.x    = rate.x;
00313   imuMsg.angular_velocity.y    = rate.y;
00314   imuMsg.angular_velocity.z    = rate.z;
00315 
00316   // pass accelerations
00317   imuMsg.linear_acceleration.x    = accel.x;
00318   imuMsg.linear_acceleration.y    = accel.y;
00319   imuMsg.linear_acceleration.z    = accel.z;
00320 
00321   // fill in covariance matrix
00322   imuMsg.orientation_covariance[8] = yawModel.gaussian_noise*yawModel.gaussian_noise;
00323   if (gravity_length > 0.0) {
00324     imuMsg.orientation_covariance[0] = accelModel.gaussian_noise.x*accelModel.gaussian_noise.x/(gravity_length*gravity_length);
00325     imuMsg.orientation_covariance[4] = accelModel.gaussian_noise.y*accelModel.gaussian_noise.y/(gravity_length*gravity_length);
00326   } else {
00327     imuMsg.orientation_covariance[0] = -1;
00328     imuMsg.orientation_covariance[4] = -1;
00329   }
00330 
00331   // publish to ros
00332   pub_.publish(imuMsg);
00333   ROS_DEBUG_NAMED("gazebo_ros_imu", "Publishing IMU data at t = %f", cur_time.Double());
00334 
00335   // publish bias
00336   if (bias_pub_) {
00337     biasMsg.header = imuMsg.header;
00338     biasMsg.orientation.x = orientationError.x;
00339     biasMsg.orientation.y = orientationError.y;
00340     biasMsg.orientation.z = orientationError.z;
00341     biasMsg.orientation.w = orientationError.w;
00342     biasMsg.angular_velocity.x = rateModel.getCurrentBias().x;
00343     biasMsg.angular_velocity.y = rateModel.getCurrentBias().y;
00344     biasMsg.angular_velocity.z = rateModel.getCurrentBias().z;
00345     biasMsg.linear_acceleration.x = accelModel.getCurrentBias().x;
00346     biasMsg.linear_acceleration.y = accelModel.getCurrentBias().y;
00347     biasMsg.linear_acceleration.z = accelModel.getCurrentBias().z;
00348     bias_pub_.publish(biasMsg);
00349   }
00350 
00351   // debug output
00352 #ifdef DEBUG_OUTPUT
00353   if (debugPublisher) {
00354     geometry_msgs::PoseStamped debugPose;
00355     debugPose.header = imuMsg.header;
00356     debugPose.header.frame_id = "/map";
00357     debugPose.pose.orientation.w = imuMsg.orientation.w;
00358     debugPose.pose.orientation.x = imuMsg.orientation.x;
00359     debugPose.pose.orientation.y = imuMsg.orientation.y;
00360     debugPose.pose.orientation.z = imuMsg.orientation.z;
00361     math::Pose pose = link->GetWorldPose();
00362     debugPose.pose.position.x = pose.pos.x;
00363     debugPose.pose.position.y = pose.pos.y;
00364     debugPose.pose.position.z = pose.pos.z;
00365     debugPublisher.publish(debugPose);
00366   }
00367 #endif // DEBUG_OUTPUT
00368 }
00369 
00370 #ifdef USE_CBQ
00371 void GazeboRosIMU::CallbackQueueThread()
00372 {
00373   static const double timeout = 0.01;
00374 
00375   while (rosnode_->ok())
00376   {
00377     callback_queue_.callAvailable(ros::WallDuration(timeout));
00378   }
00379 }
00380 #endif
00381 
00382 // Register this plugin with the simulator
00383 GZ_REGISTER_MODEL_PLUGIN(GazeboRosIMU)
00384 
00385 } // namespace gazebo


hector_gazebo_plugins
Author(s): Stefan Kohlbrecher , Johannes Meyer
autogenerated on Mon Jun 27 2016 04:58:09