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 // This code is based on the original gazebo_ros_imu plugin by Sachin Chitta and John Hsu:
00029 /*
00030  *  Gazebo - Outdoor Multi-Robot Simulator
00031  *  Copyright (C) 2003
00032  *     Nate Koenig & Andrew Howard
00033  *
00034  *  This program is free software; you can redistribute it and/or modify
00035  *  it under the terms of the GNU General Public License as published by
00036  *  the Free Software Foundation; either version 2 of the License, or
00037  *  (at your option) any later version.
00038  *
00039  *  This program is distributed in the hope that it will be useful,
00040  *  but WITHOUT ANY WARRANTY; without even the implied warranty of
00041  *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
00042  *  GNU General Public License for more details.
00043  *
00044  *  You should have received a copy of the GNU General Public License
00045  *  along with this program; if not, write to the Free Software
00046  *  Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
00047  *
00048  */
00049 /*
00050  * Desc: 3D position interface.
00051  * Author: Sachin Chitta and John Hsu
00052  * Date: 10 June 2008
00053  * SVN: $Id$
00054  */
00055 //=================================================================================================
00056 
00057 #include <hector_gazebo_plugins/gazebo_ros_imu.h>
00058 #include "common/Events.hh"
00059 #include "physics/physics.hh"
00060 
00061 #include <ros/console.h>
00062 
00063 namespace gazebo
00064 {
00065 
00066 // #define DEBUG_OUTPUT
00067 #ifdef DEBUG_OUTPUT
00068   #include <geometry_msgs/PoseStamped.h>
00069   static ros::Publisher debugPublisher;
00070 #endif // DEBUG_OUTPUT
00071 
00073 // Constructor
00074 GazeboRosIMU::GazeboRosIMU()
00075 {
00076 }
00077 
00079 // Destructor
00080 GazeboRosIMU::~GazeboRosIMU()
00081 {
00082   updateTimer.Disconnect(updateConnection);
00083 
00084   node_handle_->shutdown();
00085 #ifdef USE_CBQ
00086   callback_queue_thread_.join();
00087 #endif
00088   delete node_handle_;
00089 }
00090 
00092 // Load the controller
00093 void GazeboRosIMU::Load(physics::ModelPtr _model, sdf::ElementPtr _sdf)
00094 {
00095   // Get the world name.
00096   world = _model->GetWorld();
00097 
00098   // load parameters
00099   if (!_sdf->HasElement("robotNamespace"))
00100     robotNamespace.clear();
00101   else
00102     robotNamespace = _sdf->GetElement("robotNamespace")->GetValueString() + "/";
00103 
00104   if (!_sdf->HasElement("bodyName"))
00105   {
00106     link = _model->GetLink();
00107     linkName = link->GetName();
00108   }
00109   else {
00110     linkName = _sdf->GetElement("bodyName")->GetValueString();
00111     link = _model->GetLink(linkName);
00112   }
00113 
00114   // assert that the body by linkName exists
00115   if (!link)
00116   {
00117     ROS_FATAL("GazeboRosIMU plugin error: bodyName: %s does not exist\n", linkName.c_str());
00118     return;
00119   }
00120 
00121   if (!_sdf->HasElement("frameId"))
00122     frameId = linkName;
00123   else
00124     frameId = _sdf->GetElement("frameId")->GetValueString();
00125 
00126   if (!_sdf->HasElement("topicName"))
00127     topicName = "imu";
00128   else
00129     topicName = _sdf->GetElement("topicName")->GetValueString();
00130 
00131   if (!_sdf->HasElement("serviceName"))
00132     serviceName = topicName + "/calibrate";
00133   else
00134     serviceName = _sdf->GetElement("serviceName")->GetValueString();
00135 
00136   accelModel.Load(_sdf, "accel");
00137   rateModel.Load(_sdf, "rate");
00138   headingModel.Load(_sdf, "heading");
00139 
00140   // also use old configuration variables from gazebo_ros_imu
00141   if (_sdf->HasElement("gaussianNoise")) {
00142     double gaussianNoise = _sdf->GetElement("gaussianNoise")->GetValueDouble();
00143     if (gaussianNoise != 0.0) {
00144       accelModel.gaussian_noise = gaussianNoise;
00145       rateModel.gaussian_noise  = gaussianNoise;
00146     }
00147   }
00148 
00149   if (_sdf->HasElement("rpyOffset")) {
00150     math::Vector3 rpyOffset = _sdf->GetElement("rpyOffset")->GetValueVector3();
00151     if (accelModel.offset.y == 0.0 && rpyOffset.x != 0.0) accelModel.offset.y = -rpyOffset.x * 9.8065;
00152     if (accelModel.offset.x == 0.0 && rpyOffset.y != 0.0) accelModel.offset.x =  rpyOffset.y * 9.8065;
00153     if (headingModel.offset == 0.0 && rpyOffset.z != 0.0) headingModel.offset =  rpyOffset.z;
00154   }
00155 
00156   // fill in constant covariance matrix
00157   imuMsg.angular_velocity_covariance[0] = rateModel.gaussian_noise.x*rateModel.gaussian_noise.x;
00158   imuMsg.angular_velocity_covariance[4] = rateModel.gaussian_noise.y*rateModel.gaussian_noise.y;
00159   imuMsg.angular_velocity_covariance[8] = rateModel.gaussian_noise.z*rateModel.gaussian_noise.z;
00160   imuMsg.linear_acceleration_covariance[0] = accelModel.gaussian_noise.x*accelModel.gaussian_noise.x;
00161   imuMsg.linear_acceleration_covariance[4] = accelModel.gaussian_noise.y*accelModel.gaussian_noise.y;
00162   imuMsg.linear_acceleration_covariance[8] = accelModel.gaussian_noise.z*accelModel.gaussian_noise.z;
00163 
00164   // start ros node
00165   if (!ros::isInitialized())
00166   {
00167     int argc = 0;
00168     char** argv = NULL;
00169     ros::init(argc,argv,"gazebo",ros::init_options::NoSigintHandler|ros::init_options::AnonymousName);
00170   }
00171 
00172   node_handle_ = new ros::NodeHandle(robotNamespace);
00173 
00174   // if topic name specified as empty, do not publish (then what is this plugin good for?)
00175   if (!topicName.empty())
00176     pub_ = node_handle_->advertise<sensor_msgs::Imu>(topicName, 10);
00177 
00178 #ifdef DEBUG_OUTPUT
00179   debugPublisher = rosnode_->advertise<geometry_msgs::PoseStamped>(topicName + "/pose", 10);
00180 #endif // DEBUG_OUTPUT
00181 
00182   // advertise services for calibration and bias setting
00183   if (!serviceName.empty())
00184     srv_ = node_handle_->advertiseService(serviceName, &GazeboRosIMU::ServiceCallback, this);
00185 
00186   accelBiasService = node_handle_->advertiseService(topicName + "/set_accel_bias", &GazeboRosIMU::SetAccelBiasCallback, this);
00187   rateBiasService  = node_handle_->advertiseService(topicName + "/set_rate_bias", &GazeboRosIMU::SetRateBiasCallback, this);
00188 
00189 #ifdef USE_CBQ
00190   // start custom queue for imu
00191   callback_queue_thread_ = boost::thread( boost::bind( &GazeboRosIMU::CallbackQueueThread,this ) );
00192 #endif
00193 
00194   Reset();
00195 
00196   // connect Update function
00197   updateTimer.Load(world, _sdf);
00198   updateConnection = updateTimer.Connect(boost::bind(&GazeboRosIMU::Update, this));
00199 }
00200 
00201 void GazeboRosIMU::Reset()
00202 {
00203   updateTimer.Reset();
00204 
00205   orientation = math::Quaternion();
00206   velocity = 0.0;
00207   accel = 0.0;
00208 
00209   accelModel.reset();
00210   rateModel.reset();
00211   headingModel.reset();
00212 }
00213 
00215 // returns true always, imu is always calibrated in sim
00216 bool GazeboRosIMU::ServiceCallback(std_srvs::Empty::Request &req,
00217                                         std_srvs::Empty::Response &res)
00218 {
00219   boost::mutex::scoped_lock scoped_lock(lock);
00220   rateModel.reset();
00221   return true;
00222 }
00223 
00224 bool GazeboRosIMU::SetAccelBiasCallback(hector_gazebo_plugins::SetBias::Request &req, hector_gazebo_plugins::SetBias::Response &res)
00225 {
00226   boost::mutex::scoped_lock scoped_lock(lock);
00227   accelModel.reset(math::Vector3(req.bias.x, req.bias.y, req.bias.z));
00228   return true;
00229 }
00230 
00231 bool GazeboRosIMU::SetRateBiasCallback(hector_gazebo_plugins::SetBias::Request &req, hector_gazebo_plugins::SetBias::Response &res)
00232 {
00233   boost::mutex::scoped_lock scoped_lock(lock);
00234   rateModel.reset(math::Vector3(req.bias.x, req.bias.y, req.bias.z));
00235   return true;
00236 }
00237 
00239 // Update the controller
00240 void GazeboRosIMU::Update()
00241 {
00242   // Get Time Difference dt
00243   common::Time cur_time = world->GetSimTime();
00244   double dt = updateTimer.getTimeSinceLastUpdate().Double();
00245   boost::mutex::scoped_lock scoped_lock(lock);
00246 
00247   // Get Pose/Orientation
00248   math::Pose pose = link->GetWorldPose();
00249 
00250   // get Acceleration and Angular Rates
00251   // the result of GetRelativeLinearAccel() seems to be unreliable (sum of forces added during the current simulation step)?
00252   //accel = myBody->GetRelativeLinearAccel(); // get acceleration in body frame
00253   math::Vector3 temp = link->GetWorldLinearVel(); // get velocity in world frame
00254   if (dt > 0.0) accel = pose.rot.RotateVectorReverse((temp - velocity) / dt);
00255   velocity = temp;
00256 
00257   // GetRelativeAngularVel() sometimes return nan?
00258   //rate  = link->GetRelativeAngularVel(); // get angular rate in body frame
00259   math::Quaternion delta = pose.rot - orientation;
00260   orientation = pose.rot;
00261   if (dt > 0.0) {
00262     rate.x = 2.0 * (-orientation.x * delta.w + orientation.w * delta.x + orientation.z * delta.y - orientation.y * delta.z) / dt;
00263     rate.y = 2.0 * (-orientation.y * delta.w - orientation.z * delta.x + orientation.w * delta.y + orientation.x * delta.z) / dt;
00264     rate.z = 2.0 * (-orientation.z * delta.w + orientation.y * delta.x - orientation.x * delta.y + orientation.w * delta.z) / dt;
00265   }
00266 
00267   // get Gravity
00268   gravity       = world->GetPhysicsEngine()->GetGravity();
00269   gravity_body  = orientation.RotateVectorReverse(gravity);
00270   double gravity_length = gravity.GetLength();
00271   ROS_DEBUG_NAMED("gazebo_ros_imu", "gravity_world = [%g %g %g]", gravity.x, gravity.y, gravity.z);
00272 
00273   // add gravity vector to body acceleration
00274   accel = accel - gravity_body;
00275 
00276   // update sensor models
00277   accel = accel + accelModel.update(dt);
00278   rate  = rate  + rateModel.update(dt);
00279   headingModel.update(dt);
00280   ROS_DEBUG_NAMED("gazebo_ros_imu", "Current errors: accel = [%g %g %g], rate = [%g %g %g], heading = %g",
00281                  accelModel.getCurrentError().x, accelModel.getCurrentError().y, accelModel.getCurrentError().z,
00282                  rateModel.getCurrentError().x, rateModel.getCurrentError().y, rateModel.getCurrentError().z,
00283                  headingModel.getCurrentError());
00284 
00285   // apply offset error to orientation (pseudo AHRS)
00286   double normalization_constant = (gravity_body + accelModel.getCurrentError()).GetLength() * gravity_body.GetLength();
00287   double cos_alpha = (gravity_body + accelModel.getCurrentError()).GetDotProd(gravity_body)/normalization_constant;
00288   math::Vector3 normal_vector(gravity_body.GetCrossProd(accelModel.getCurrentError()));
00289   normal_vector *= sqrt((1 - cos_alpha)/2)/normalization_constant;
00290   math::Quaternion attitudeError(sqrt((1 + cos_alpha)/2), normal_vector.x, normal_vector.y, normal_vector.z);
00291   math::Quaternion headingError(cos(headingModel.getCurrentError()/2),0,0,sin(headingModel.getCurrentError()/2));
00292   pose.rot = attitudeError * pose.rot * headingError;
00293 
00294   // copy data into pose message
00295   imuMsg.header.frame_id = frameId;
00296   imuMsg.header.stamp.sec = cur_time.sec;
00297   imuMsg.header.stamp.nsec = cur_time.nsec;
00298 
00299   // orientation quaternion
00300   imuMsg.orientation.x = pose.rot.x;
00301   imuMsg.orientation.y = pose.rot.y;
00302   imuMsg.orientation.z = pose.rot.z;
00303   imuMsg.orientation.w = pose.rot.w;
00304 
00305   // pass angular rates
00306   imuMsg.angular_velocity.x    = rate.x;
00307   imuMsg.angular_velocity.y    = rate.y;
00308   imuMsg.angular_velocity.z    = rate.z;
00309 
00310   // pass accelerations
00311   imuMsg.linear_acceleration.x    = accel.x;
00312   imuMsg.linear_acceleration.y    = accel.y;
00313   imuMsg.linear_acceleration.z    = accel.z;
00314 
00315   // fill in covariance matrix
00316   imuMsg.orientation_covariance[8] = headingModel.gaussian_noise*headingModel.gaussian_noise;
00317   if (gravity_length > 0.0) {
00318     imuMsg.orientation_covariance[0] = accelModel.gaussian_noise.x*accelModel.gaussian_noise.x/(gravity_length*gravity_length);
00319     imuMsg.orientation_covariance[4] = accelModel.gaussian_noise.y*accelModel.gaussian_noise.y/(gravity_length*gravity_length);
00320   } else {
00321     imuMsg.orientation_covariance[0] = -1;
00322     imuMsg.orientation_covariance[4] = -1;
00323   }
00324 
00325   // publish to ros
00326   pub_.publish(imuMsg);
00327   ROS_DEBUG_NAMED("gazebo_ros_imu", "Publishing IMU data at t = %f", cur_time.Double());
00328 
00329   // debug output
00330 #ifdef DEBUG_OUTPUT
00331   if (debugPublisher) {
00332     geometry_msgs::PoseStamped debugPose;
00333     debugPose.header = imuMsg.header;
00334     debugPose.header.frame_id = "/map";
00335     debugPose.pose.orientation.w = imuMsg.orientation.w;
00336     debugPose.pose.orientation.x = imuMsg.orientation.x;
00337     debugPose.pose.orientation.y = imuMsg.orientation.y;
00338     debugPose.pose.orientation.z = imuMsg.orientation.z;
00339     math::Pose pose = link->GetWorldPose();
00340     debugPose.pose.position.x = pose.pos.x;
00341     debugPose.pose.position.y = pose.pos.y;
00342     debugPose.pose.position.z = pose.pos.z;
00343     debugPublisher.publish(debugPose);
00344   }
00345 #endif // DEBUG_OUTPUT
00346 }
00347 
00348 #ifdef USE_CBQ
00349 void GazeboRosIMU::CallbackQueueThread()
00350 {
00351   static const double timeout = 0.01;
00352 
00353   while (rosnode_->ok())
00354   {
00355     callback_queue_.callAvailable(ros::WallDuration(timeout));
00356   }
00357 }
00358 #endif
00359 
00360 // Register this plugin with the simulator
00361 GZ_REGISTER_MODEL_PLUGIN(GazeboRosIMU)
00362 
00363 } // namespace gazebo
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Properties Friends


hector_gazebo_plugins
Author(s): Stefan Kohlbrecher and Johannes Meyer
autogenerated on Mon Jul 15 2013 16:46:30