hector_gazebo_plugins: hector_gazebo_plugins: gazebo_ros

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 
00059 #include <gazebo/Global.hh>
00060 #include <gazebo/XMLConfig.hh>
00061 #include <gazebo/Simulator.hh>
00062 #include <gazebo/gazebo.h>
00063 #include <gazebo/GazeboError.hh>
00064 #include <gazebo/ControllerFactory.hh>
00065 
00066 #include <gazebo/World.hh>
00067 #include <gazebo/PhysicsEngine.hh>
00068 
00069 using namespace gazebo;
00070 
00071 GZ_REGISTER_DYNAMIC_CONTROLLER("hector_gazebo_ros_imu", GazeboRosIMU);
00072 
00073 // #define DEBUG_OUTPUT
00074 #ifdef DEBUG_OUTPUT
00075   #include <geometry_msgs/PoseStamped.h>
00076   static ros::Publisher debugPublisher;
00077 #endif // DEBUG_OUTPUT
00078 
00080 // Constructor
00081 GazeboRosIMU::GazeboRosIMU(Entity *parent)
00082    : Controller(parent)
00083    , accelModel(parameters, "accel")
00084    , rateModel(parameters, "rate")
00085    , headingModel(parameters, "heading")
00086 {
00087    myParent = dynamic_cast<Model*>(parent);
00088 
00089    if (!myParent)
00090       gzthrow("GazeboRosIMU controller requires a Model as its parent");
00091 
00092   Param::Begin(&parameters);
00093   robotNamespaceP = new ParamT<std::string>("robotNamespace", "", false);
00094   bodyNameP = new ParamT<std::string>("bodyName", "", true);
00095   topicNameP = new ParamT<std::string>("topicName", "imu", false);
00096   rpyOffsetsP    = new ParamT<Vector3>("rpyOffsets", Vector3(0,0,0), false);
00097   gaussianNoiseP = new ParamT<double>("gaussianNoise",0.0, false);
00098   serviceNameP = new ParamT<std::string>("serviceName", "", false);
00099   Param::End();
00100 }
00101 
00103 // Destructor
00104 GazeboRosIMU::~GazeboRosIMU()
00105 {
00106   delete robotNamespaceP;
00107   delete bodyNameP;
00108   delete topicNameP;
00109   delete rpyOffsetsP;
00110   delete gaussianNoiseP;
00111   delete serviceNameP;
00112   delete rosnode_;
00113 }
00114 
00116 // Load the controller
00117 void GazeboRosIMU::LoadChild(XMLConfigNode *node)
00118 {
00119   robotNamespaceP->Load(node);
00120   robotNamespace = robotNamespaceP->GetValue();
00121   if (!ros::isInitialized())
00122   {
00123     int argc = 0;
00124     char** argv = NULL;
00125     ros::init(argc,argv, "gazebo", ros::init_options::NoSigintHandler|ros::init_options::AnonymousName);
00126   }
00127 
00128   rosnode_ = new ros::NodeHandle(robotNamespace);
00129 #ifdef USE_CBQ
00130   rosnode_->setCallbackQueue(&callback_queue_);
00131 #endif
00132 
00133   bodyNameP->Load(node);
00134   bodyName = bodyNameP->GetValue();
00135 
00136   // assert that the body by bodyName exists
00137   if (dynamic_cast<Body*>(myParent->GetBody(bodyName)) == NULL)
00138     ROS_FATAL("gazebo_ros_imu plugin error: bodyName: %s does not exist\n",bodyName.c_str());
00139 
00140   myBody = dynamic_cast<Body*>(myParent->GetBody(bodyName));
00141 
00142   topicNameP->Load(node);
00143   topicName = topicNameP->GetValue();
00144 
00145   accelModel.Load(node);
00146   rateModel.Load(node);
00147   headingModel.Load(node);
00148 
00149   // also use old configuration variables from gazebo_ros_imu
00150   rpyOffsetsP->Load(node);
00151   Vector3 rpyOffsets = rpyOffsetsP->GetValue();
00152   if (accelModel.offset.y == 0.0 && rpyOffsets.x != 0.0) accelModel.offset.y = -rpyOffsets.x * 9.8065;
00153   if (accelModel.offset.x == 0.0 && rpyOffsets.y != 0.0) accelModel.offset.x =  rpyOffsets.y * 9.8065;
00154   if (headingModel.offset == 0.0 && rpyOffsets.z != 0.0) headingModel.offset =  rpyOffsets.z;
00155   gaussianNoiseP->Load(node);
00156   double gaussianNoise = gaussianNoiseP->GetValue();
00157   if (gaussianNoise != 0.0) {
00158     accelModel.gaussian_noise = gaussianNoise;
00159     rateModel.gaussian_noise  = gaussianNoise;
00160   }
00161 
00162   // fill in constant covariance matrix
00163   imuMsg.angular_velocity_covariance[0] = rateModel.gaussian_noise.x*rateModel.gaussian_noise.x;
00164   imuMsg.angular_velocity_covariance[4] = rateModel.gaussian_noise.y*rateModel.gaussian_noise.y;
00165   imuMsg.angular_velocity_covariance[8] = rateModel.gaussian_noise.z*rateModel.gaussian_noise.z;
00166   imuMsg.linear_acceleration_covariance[0] = accelModel.gaussian_noise.x*accelModel.gaussian_noise.x;
00167   imuMsg.linear_acceleration_covariance[4] = accelModel.gaussian_noise.y*accelModel.gaussian_noise.y;
00168   imuMsg.linear_acceleration_covariance[8] = accelModel.gaussian_noise.z*accelModel.gaussian_noise.z;
00169 
00170   if (topicName != "")
00171   {
00172     pub_ = rosnode_->advertise<sensor_msgs::Imu>(topicName,10);
00173   }
00174 
00175 #ifdef DEBUG_OUTPUT
00176   debugPublisher = rosnode_->advertise<geometry_msgs::PoseStamped>(topicName+"/pose", 10);
00177 #endif // DEBUG_OUTPUT
00178 
00179   // advertise services for calibration and bias setting
00180   serviceNameP->Load(node);
00181   serviceName = serviceNameP->GetValue();
00182   if (serviceName.empty()) serviceName = topicName+"/calibrate";
00183   srv_ = rosnode_->advertiseService(serviceName, &GazeboRosIMU::ServiceCallback, this);
00184   accelBiasService = rosnode_->advertiseService(topicName+"/set_accel_bias", &GazeboRosIMU::SetAccelBiasCallback, this);
00185   rateBiasService  = rosnode_->advertiseService(topicName+"/set_rate_bias", &GazeboRosIMU::SetRateBiasCallback, this);
00186 }
00187 
00189 // returns true always, imu is always calibrated in sim
00190 bool GazeboRosIMU::ServiceCallback(std_srvs::Empty::Request &req,
00191                                         std_srvs::Empty::Response &res)
00192 {
00193   lock.lock();
00194   rateModel.reset();
00195   lock.unlock();
00196   return true;
00197 }
00198 
00199 bool GazeboRosIMU::SetAccelBiasCallback(hector_gazebo_plugins::SetBias::Request &req, hector_gazebo_plugins::SetBias::Response &res)
00200 {
00201   lock.lock();
00202   accelModel.reset(Vector3(req.bias.x, req.bias.y, req.bias.z));
00203   lock.unlock();
00204   return true;
00205 }
00206 
00207 bool GazeboRosIMU::SetRateBiasCallback(hector_gazebo_plugins::SetBias::Request &req, hector_gazebo_plugins::SetBias::Response &res)
00208 {
00209   lock.lock();
00210   rateModel.reset(Vector3(req.bias.x, req.bias.y, req.bias.z));
00211   lock.unlock();
00212   return true;
00213 }
00214 
00216 // Initialize the controller
00217 void GazeboRosIMU::InitChild()
00218 {
00219   orientation = Quatern();
00220   velocity = 0.0;
00221   accel = 0.0;
00222 
00223 #ifdef USE_CBQ
00224   // start custom queue for imu
00225   callback_queue_thread_ = boost::thread( boost::bind( &GazeboRosIMU::CallbackQueueThread,this ) );
00226 #endif
00227 }
00228 
00230 // Update the controller
00231 void GazeboRosIMU::UpdateChild()
00232 {
00233   // Get Pose/Orientation
00234   Pose3d pose = myBody->GetWorldPose();
00235 
00236   // Get Time Difference dt
00237   gazebo::Time cur_time = Simulator::Instance()->GetSimTime();
00238   double dt = cur_time - lastUpdate;
00239 
00240   lock.lock();
00241 
00242   // get Acceleration and Angular Rates
00243   // the result of GetRelativeLinearAccel() seems to be unreliable (sum of forces added during the current simulation step)?
00244   //accel = myBody->GetRelativeLinearAccel(); // get acceleration in body frame
00245   Vector3 temp = myBody->GetWorldLinearVel(); // get velocity in world frame
00246   accel = pose.rot.RotateVectorReverse((temp - velocity) / dt);
00247   velocity = temp;
00248 
00249   // GetRelativeAngularVel() sometimes return nan?
00250   //rate  = myBody->GetRelativeAngularVel(); // get angular rate in body frame
00251   Quatern delta = pose.rot - orientation;
00252   orientation = pose.rot;
00253   rate.x = 2.0 * (-orientation.x * delta.u + orientation.u * delta.x + orientation.z * delta.y - orientation.y * delta.z) / dt;
00254   rate.y = 2.0 * (-orientation.y * delta.u - orientation.z * delta.x + orientation.u * delta.y + orientation.x * delta.z) / dt;
00255   rate.z = 2.0 * (-orientation.z * delta.u + orientation.y * delta.x - orientation.x * delta.y + orientation.u * delta.z) / dt;
00256 
00257   // get Gravity
00258   gravity       = World::Instance()->GetPhysicsEngine()->GetGravity();
00259   gravity_body  = orientation.RotateVectorReverse(gravity);
00260   double gravity_length = gravity.GetLength();
00261   ROS_DEBUG_NAMED("hector_gazebo_ros_imu", "gravity_world = [%g %g %g]", gravity.x, gravity.y, gravity.z);
00262 
00263   // add gravity vector to body acceleration
00264   accel = accel - gravity_body;
00265 
00266   // update sensor models
00267   accel = accel + accelModel.update(dt);
00268   rate  = rate  + rateModel.update(dt);
00269   headingModel.update(dt);
00270   ROS_DEBUG_NAMED("hector_gazebo_ros_imu", "Current errors: accel = [%g %g %g], rate = [%g %g %g], heading = %g",
00271                  accelModel.getCurrentError().x, accelModel.getCurrentError().y, accelModel.getCurrentError().z,
00272                  rateModel.getCurrentError().x, rateModel.getCurrentError().y, rateModel.getCurrentError().z,
00273                  headingModel.getCurrentError());
00274 
00275   // apply offset error to orientation (pseudo AHRS)
00276   double normalization_constant = (gravity_body + accelModel.getCurrentError()).GetLength() * gravity_body.GetLength();
00277   double cos_alpha = (gravity_body + accelModel.getCurrentError()).GetDotProd(gravity_body)/normalization_constant;
00278   Vector3 normal_vector(gravity_body.GetCrossProd(accelModel.getCurrentError()));
00279   normal_vector *= sqrt((1 - cos_alpha)/2)/normalization_constant;
00280   Quatern attitudeError(sqrt((1 + cos_alpha)/2), normal_vector.x, normal_vector.y, normal_vector.z);
00281   Quatern headingError(cos(headingModel.getCurrentError()/2),0,0,sin(headingModel.getCurrentError()/2));
00282   pose.rot = attitudeError * pose.rot * headingError;
00283 
00284   // copy data into pose message
00285   imuMsg.header.frame_id = bodyName;
00286   imuMsg.header.stamp.sec = cur_time.sec;
00287   imuMsg.header.stamp.nsec = cur_time.nsec;
00288 
00289   // orientation quaternion
00290   imuMsg.orientation.x = pose.rot.x;
00291   imuMsg.orientation.y = pose.rot.y;
00292   imuMsg.orientation.z = pose.rot.z;
00293   imuMsg.orientation.w = pose.rot.u;
00294 
00295   // pass angular rates
00296   imuMsg.angular_velocity.x    = rate.x;
00297   imuMsg.angular_velocity.y    = rate.y;
00298   imuMsg.angular_velocity.z    = rate.z;
00299 
00300   // pass accelerations
00301   imuMsg.linear_acceleration.x    = accel.x;
00302   imuMsg.linear_acceleration.y    = accel.y;
00303   imuMsg.linear_acceleration.z    = accel.z;
00304 
00305   // fill in covariance matrix
00306   imuMsg.orientation_covariance[8] = headingModel.gaussian_noise*headingModel.gaussian_noise;
00307   if (gravity_length > 0.0) {
00308     imuMsg.orientation_covariance[0] = accelModel.gaussian_noise.x*accelModel.gaussian_noise.x/(gravity_length*gravity_length);
00309     imuMsg.orientation_covariance[4] = accelModel.gaussian_noise.y*accelModel.gaussian_noise.y/(gravity_length*gravity_length);
00310   } else {
00311     imuMsg.orientation_covariance[0] = -1;
00312     imuMsg.orientation_covariance[4] = -1;
00313   }
00314 
00315   // publish to ros
00316   pub_.publish(imuMsg);
00317 
00318   // debug output
00319 #ifdef DEBUG_OUTPUT
00320   if (debugPublisher) {
00321     geometry_msgs::PoseStamped debugPose;
00322     debugPose.header = imuMsg.header;
00323     debugPose.header.frame_id = "/map";
00324     debugPose.pose.orientation.w = imuMsg.orientation.w;
00325     debugPose.pose.orientation.x = imuMsg.orientation.x;
00326     debugPose.pose.orientation.y = imuMsg.orientation.y;
00327     debugPose.pose.orientation.z = imuMsg.orientation.z;
00328     Pose3d pose = myBody->GetWorldPose();
00329     debugPose.pose.position.x = pose.pos.x;
00330     debugPose.pose.position.y = pose.pos.y;
00331     debugPose.pose.position.z = pose.pos.z;
00332     debugPublisher.publish(debugPose);
00333   }
00334 #endif // DEBUG_OUTPUT
00335 
00336   lock.unlock();
00337 }
00338 
00340 // Finalize the controller
00341 void GazeboRosIMU::FiniChild()
00342 {
00343   rosnode_->shutdown();
00344   callback_queue_thread_.join();
00345 }
00346 
00347 #ifdef USE_CBQ
00348 void GazeboRosIMU::CallbackQueueThread()
00349 {
00350   static const double timeout = 0.01;
00351 
00352   while (rosnode_->ok())
00353   {
00354     callback_queue_.callAvailable(ros::WallDuration(timeout));
00355   }
00356 }
00357 #endif
00358
gazebo_ros_imu.cpp
