pose_estimation_node.cpp
Go to the documentation of this file.
00001 //=================================================================================================
00002 // Copyright (c) 2011, 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_pose_estimation/pose_estimation_node.h>
00030 #include <hector_pose_estimation/ros/parameters.h>
00031 
00032 #include <hector_pose_estimation/system/generic_quaternion_system_model.h>
00033 #include <hector_pose_estimation/system/imu_input.h>
00034 #include <hector_pose_estimation/system/imu_model.h>
00035 #include <hector_pose_estimation/measurements/poseupdate.h>
00036 #include <hector_pose_estimation/measurements/baro.h>
00037 #include <hector_pose_estimation/measurements/height.h>
00038 #include <hector_pose_estimation/measurements/magnetic.h>
00039 #include <hector_pose_estimation/measurements/gps.h>
00040 
00041 #ifdef USE_HECTOR_TIMING
00042   #include <hector_diagnostics/timing.h>
00043 #endif
00044 
00045 namespace hector_pose_estimation {
00046 
00047 PoseEstimationNode::PoseEstimationNode(const SystemPtr& system, const StatePtr& state)
00048   : pose_estimation_(new PoseEstimation(system, state))
00049   , private_nh_("~")
00050   , transform_listener_(0)
00051   , world_nav_transform_updated_(true)
00052   , world_nav_transform_valid_(false)
00053   , sensor_pose_roll_(0), sensor_pose_pitch_(0), sensor_pose_yaw_(0)
00054 {
00055   if (!system) pose_estimation_->addSystem(new GenericQuaternionSystemModel);
00056 
00057   pose_estimation_->addInput(new ImuInput, "imu");
00058   pose_estimation_->addMeasurement(new PoseUpdate("poseupdate"));
00059 #if defined(USE_HECTOR_UAV_MSGS)
00060   pose_estimation_->addMeasurement(new Baro("baro"));
00061 #endif
00062   pose_estimation_->addMeasurement(new Height("height"));
00063   pose_estimation_->addMeasurement(new Magnetic("magnetic"));
00064   pose_estimation_->addMeasurement(new GPS("gps"));
00065 }
00066 
00067 PoseEstimationNode::~PoseEstimationNode()
00068 {
00069   cleanup();
00070   delete pose_estimation_;
00071   delete transform_listener_;
00072 }
00073 
00074 bool PoseEstimationNode::init() {
00075   // get parameters
00076   pose_estimation_->parameters().initialize(ParameterRegistryROS(getPrivateNodeHandle()));
00077   getPrivateNodeHandle().getParam("publish_covariances", publish_covariances_ = false);
00078   if (getPrivateNodeHandle().getParam("publish_world_map_transform", publish_world_other_transform_ = false)) {
00079     ROS_WARN("Parameter 'publish_world_map_transform' is deprecated. Use 'publish_world_other_transform' and 'other_frame' instead.");
00080   }
00081   getPrivateNodeHandle().getParam("publish_world_other_transform", publish_world_other_transform_);
00082   if (getPrivateNodeHandle().getParam("map_frame", other_frame_ = std::string())) {
00083     ROS_WARN("Parameter 'map_frame' is deprecated. Use 'other_frame' instead.");
00084   }
00085   getPrivateNodeHandle().getParam("other_frame", other_frame_);
00086   getPrivateNodeHandle().getParam("publish_world_nav_transform", publish_world_nav_transform_ = false);
00087 
00088   // search tf_prefix parameter
00089   tf_prefix_ = tf::getPrefixParam(getPrivateNodeHandle());
00090   if (!tf_prefix_.empty()) ROS_INFO("Using tf_prefix '%s'", tf_prefix_.c_str());
00091 
00092   // initialize pose estimation
00093   if (!pose_estimation_->init()) {
00094     ROS_ERROR("Intitialization of pose estimation failed!");
00095     return false;
00096   }
00097 
00098   imu_subscriber_        = getNodeHandle().subscribe("raw_imu", 10, &PoseEstimationNode::imuCallback, this);
00099   ahrs_subscriber_       = getNodeHandle().subscribe("ahrs", 10, &PoseEstimationNode::ahrsCallback, this);
00100   rollpitch_subscriber_  = getNodeHandle().subscribe("rollpitch", 10, &PoseEstimationNode::rollpitchCallback, this);
00101 #if defined(USE_HECTOR_UAV_MSGS)
00102   baro_subscriber_       = getNodeHandle().subscribe("altimeter", 10, &PoseEstimationNode::baroCallback, this);
00103 #else
00104   height_subscriber_     = getNodeHandle().subscribe("pressure_height", 10, &PoseEstimationNode::heightCallback, this);
00105 #endif
00106   magnetic_subscriber_   = getNodeHandle().subscribe("magnetic", 10, &PoseEstimationNode::magneticCallback, this);
00107 
00108   gps_subscriber_.subscribe(getNodeHandle(), "fix", 10);
00109   gps_velocity_subscriber_.subscribe(getNodeHandle(), "fix_velocity", 10);
00110   gps_synchronizer_ = new message_filters::TimeSynchronizer<sensor_msgs::NavSatFix,geometry_msgs::Vector3Stamped>(gps_subscriber_, gps_velocity_subscriber_, 10);
00111   gps_synchronizer_->registerCallback(&PoseEstimationNode::gpsCallback, this);
00112 
00113   state_publisher_       = getNodeHandle().advertise<nav_msgs::Odometry>("state", 10, false);
00114   pose_publisher_        = getNodeHandle().advertise<geometry_msgs::PoseStamped>("pose", 10, false);
00115   velocity_publisher_    = getNodeHandle().advertise<geometry_msgs::Vector3Stamped>("velocity", 10, false);
00116   imu_publisher_         = getNodeHandle().advertise<sensor_msgs::Imu>("imu", 10, false);
00117   geopose_publisher_     = getNodeHandle().advertise<geographic_msgs::GeoPose>("geopose", 10, false);
00118   global_fix_publisher_  = getNodeHandle().advertise<sensor_msgs::NavSatFix>("global", 10, false);
00119   euler_publisher_       = getNodeHandle().advertise<geometry_msgs::Vector3Stamped>("euler", 10, false);
00120 
00121   angular_velocity_bias_publisher_    = getNodeHandle().advertise<geometry_msgs::Vector3Stamped>("angular_velocity_bias", 10, false);
00122   linear_acceleration_bias_publisher_ = getNodeHandle().advertise<geometry_msgs::Vector3Stamped>("linear_acceleration_bias", 10, false);
00123   gps_pose_publisher_                 = getNodeHandle().advertise<geometry_msgs::PoseStamped>("fix/pose", 10, false);
00124   sensor_pose_publisher_              = getNodeHandle().advertise<geometry_msgs::PoseStamped>("sensor_pose", 10, false);
00125 
00126   poseupdate_subscriber_  = getNodeHandle().subscribe("poseupdate", 10, &PoseEstimationNode::poseupdateCallback, this);
00127   twistupdate_subscriber_ = getNodeHandle().subscribe("twistupdate", 10, &PoseEstimationNode::twistupdateCallback, this);
00128   syscommand_subscriber_  = getNodeHandle().subscribe("syscommand", 10, &PoseEstimationNode::syscommandCallback, this);
00129 
00130 #ifdef USE_HECTOR_TIMING
00131   timing_publisher_ = getPrivateNodeHandle().advertise<hector_diagnostics::TimingAggregator>("timing", 10, false);
00132 #endif
00133 
00134   global_reference_publisher_  = getNodeHandle().advertise<geographic_msgs::GeoPose>("global/reference", 1, true);
00135   pose_estimation_->globalReference()->addUpdateCallback(boost::bind(&PoseEstimationNode::globalReferenceUpdated, this));
00136 
00137   // setup publish_world_nav_transform timer
00138   if (publish_world_nav_transform_) {
00139     double period = 0.1;
00140     getPrivateNodeHandle().getParam("publish_world_nav_transform_period", period);
00141     publish_world_nav_transform_period_ = ros::Duration(period);
00142     publish_world_nav_transform_timer_ = getNodeHandle().createTimer(publish_world_nav_transform_period_, &PoseEstimationNode::publishWorldNavTransform, this,
00143                                                                      /* oneshot = */ false, /* autostart = */ true);
00144   }
00145 
00146   // publish initial state
00147   publish();
00148 
00149   return true;
00150 }
00151 
00152 void PoseEstimationNode::reset() {
00153   pose_estimation_->reset();
00154 
00155   sensor_pose_ = geometry_msgs::PoseStamped();
00156   sensor_pose_roll_  = 0.0;
00157   sensor_pose_pitch_ = 0.0;
00158   sensor_pose_yaw_   = 0.0;
00159 }
00160 
00161 void PoseEstimationNode::cleanup() {
00162   if (gps_synchronizer_) {
00163     delete gps_synchronizer_;
00164     gps_synchronizer_ = 0;
00165   }
00166   publish_world_nav_transform_timer_.stop();
00167 
00168   pose_estimation_->cleanup();
00169 }
00170 
00171 void PoseEstimationNode::imuCallback(const sensor_msgs::ImuConstPtr& imu) {
00172   pose_estimation_->setInput(ImuInput(*imu));
00173   pose_estimation_->update(imu->header.stamp);
00174 
00175   // calculate roll and pitch purely from acceleration
00176   if (sensor_pose_publisher_) {
00177     tf::Vector3 linear_acceleration_body(imu->linear_acceleration.x, imu->linear_acceleration.y, imu->linear_acceleration.z);
00178     linear_acceleration_body.normalize();
00179     sensor_pose_roll_  =  atan2(linear_acceleration_body.y(), linear_acceleration_body.z());
00180     sensor_pose_pitch_ = -asin(linear_acceleration_body.x());
00181   }
00182 
00183   publish();
00184 }
00185 
00186 void PoseEstimationNode::ahrsCallback(const sensor_msgs::ImuConstPtr& ahrs) {
00187   pose_estimation_->state().setOrientation(Quaternion(ahrs->orientation.w, ahrs->orientation.x, ahrs->orientation.y, ahrs->orientation.z));
00188   pose_estimation_->setInput(ImuInput(*ahrs));
00189   pose_estimation_->update(ahrs->header.stamp);
00190   publish();
00191 }
00192 
00193 void PoseEstimationNode::rollpitchCallback(const sensor_msgs::ImuConstPtr& attitude) {
00194   pose_estimation_->state().setRollPitch(Quaternion(attitude->orientation.w, attitude->orientation.x, attitude->orientation.y, attitude->orientation.z));
00195   pose_estimation_->setInput(ImuInput(*attitude));
00196   pose_estimation_->update(attitude->header.stamp);
00197   publish();
00198 }
00199 
00200 #if defined(USE_HECTOR_UAV_MSGS)
00201 void PoseEstimationNode::baroCallback(const hector_uav_msgs::AltimeterConstPtr& altimeter) {
00202   boost::shared_ptr<Baro> m = boost::static_pointer_cast<Baro>(pose_estimation_->getMeasurement("baro"));
00203   m->add(Baro::Update(altimeter->pressure, altimeter->qnh));
00204 }
00205 
00206 #else
00207 void PoseEstimationNode::heightCallback(const geometry_msgs::PointStampedConstPtr& height) {
00208   boost::shared_ptr<Height> m = boost::static_pointer_cast<Height>(pose_estimation_->getMeasurement("height"));
00209 
00210   Height::MeasurementVector update;
00211   update(0) = height->point.z;
00212   m->add(Height::Update(update));
00213 
00214   if (sensor_pose_publisher_) {
00215     sensor_pose_.pose.position.z = height->point.z - m->getElevation();
00216   }
00217 }
00218 #endif
00219 
00220 void PoseEstimationNode::magneticCallback(const geometry_msgs::Vector3StampedConstPtr& magnetic) {
00221   boost::shared_ptr<Magnetic> m = boost::static_pointer_cast<Magnetic>(pose_estimation_->getMeasurement("magnetic"));
00222 
00223   Magnetic::MeasurementVector update;
00224   update.x() = magnetic->vector.x;
00225   update.y() = magnetic->vector.y;
00226   update.z() = magnetic->vector.z;
00227   m->add(Magnetic::Update(update));
00228 
00229   if (sensor_pose_publisher_) {
00230     sensor_pose_yaw_ = -(m->getModel()->getTrueHeading(pose_estimation_->state(), update) - pose_estimation_->globalReference()->heading());
00231   }
00232 }
00233 
00234 void PoseEstimationNode::gpsCallback(const sensor_msgs::NavSatFixConstPtr& gps, const geometry_msgs::Vector3StampedConstPtr& gps_velocity) {
00235   boost::shared_ptr<GPS> m = boost::static_pointer_cast<GPS>(pose_estimation_->getMeasurement("gps"));
00236 
00237   if (gps->status.status == sensor_msgs::NavSatStatus::STATUS_NO_FIX) {
00238     if (m->getStatusFlags() > 0) m->reset(pose_estimation_->state());
00239     return;
00240   }
00241 
00242   GPS::Update update;
00243   update.latitude = gps->latitude * M_PI/180.0;
00244   update.longitude = gps->longitude * M_PI/180.0;
00245   update.velocity_north =  gps_velocity->vector.x;
00246   update.velocity_east  = -gps_velocity->vector.y;
00247   m->add(update);
00248 
00249   if (gps_pose_publisher_ || sensor_pose_publisher_) {
00250     geometry_msgs::PoseStamped gps_pose;
00251     pose_estimation_->getHeader(gps_pose.header);
00252     gps_pose.header.stamp = gps->header.stamp;
00253     GPSModel::MeasurementVector y = m->getVector(update, pose_estimation_->state());
00254 
00255     if (gps_pose_publisher_) {
00256       gps_pose.pose.position.x = y(0);
00257       gps_pose.pose.position.y = y(1);
00258       gps_pose.pose.position.z = gps->altitude - pose_estimation_->globalReference()->position().altitude;
00259       double track = atan2(gps_velocity->vector.y, gps_velocity->vector.x);
00260       gps_pose.pose.orientation.w = cos(track/2);
00261       gps_pose.pose.orientation.z = sin(track/2);
00262       gps_pose_publisher_.publish(gps_pose);
00263     }
00264 
00265     sensor_pose_.pose.position.x = y(0);
00266     sensor_pose_.pose.position.y = y(1);
00267   }
00268 }
00269 
00270 void PoseEstimationNode::poseupdateCallback(const geometry_msgs::PoseWithCovarianceStampedConstPtr& pose) {
00271   pose_estimation_->getMeasurement("poseupdate")->add(PoseUpdate::Update(pose));
00272 
00273   if (sensor_pose_publisher_) {
00274     if (pose->pose.covariance[0] > 0)  sensor_pose_.pose.position.x = pose->pose.pose.position.x;
00275     if (pose->pose.covariance[7] > 0)  sensor_pose_.pose.position.y = pose->pose.pose.position.y;
00276     if (pose->pose.covariance[14] > 0) sensor_pose_.pose.position.z = pose->pose.pose.position.z;
00277     tf::Quaternion q;
00278     double yaw, pitch, roll;
00279     tf::quaternionMsgToTF(pose->pose.pose.orientation, q);
00280     tf::Matrix3x3(q).getEulerYPR(yaw, pitch, roll);
00281     if (pose->pose.covariance[21] > 0) sensor_pose_roll_  = roll;
00282     if (pose->pose.covariance[28] > 0) sensor_pose_pitch_ = pitch;
00283     if (pose->pose.covariance[35] > 0) sensor_pose_yaw_   = yaw;
00284   }
00285 }
00286 
00287 void PoseEstimationNode::twistupdateCallback(const geometry_msgs::TwistWithCovarianceStampedConstPtr& twist) {
00288   pose_estimation_->getMeasurement("poseupdate")->add(PoseUpdate::Update(twist));
00289 }
00290 
00291 void PoseEstimationNode::syscommandCallback(const std_msgs::StringConstPtr& syscommand) {
00292   if (syscommand->data == "reset") {
00293     ROS_INFO("Resetting pose_estimation");
00294     pose_estimation_->reset();
00295     publish();
00296   }
00297 }
00298 
00299 void PoseEstimationNode::globalReferenceUpdated() {
00300   geographic_msgs::GeoPose geopose;
00301   pose_estimation_->globalReference()->getGeoPose(geopose);
00302   global_reference_publisher_.publish(geopose);
00303 
00304   // update world nav transform
00305   world_nav_transform_updated_ = true;
00306 }
00307 
00308 void PoseEstimationNode::publishWorldNavTransform(const ros::TimerEvent &) {
00309   if (world_nav_transform_updated_) {
00310     world_nav_transform_valid_ = pose_estimation_->getWorldToNavTransform(world_nav_transform_);
00311     world_nav_transform_updated_ = false;
00312   }
00313 
00314   if (world_nav_transform_valid_) {
00315     world_nav_transform_.header.stamp = ros::Time::now() + publish_world_nav_transform_period_;
00316     getTransformBroadcaster()->sendTransform(world_nav_transform_);
00317   }
00318 }
00319 
00320 void PoseEstimationNode::publish() {
00321   if (state_publisher_ && state_publisher_.getNumSubscribers() > 0) {
00322     nav_msgs::Odometry state;
00323     pose_estimation_->getState(state, publish_covariances_);
00324     state_publisher_.publish(state);
00325   }
00326 
00327   if (pose_publisher_ && pose_publisher_.getNumSubscribers() > 0) {
00328     geometry_msgs::PoseStamped pose_msg;
00329     pose_estimation_->getPose(pose_msg);
00330     pose_publisher_.publish(pose_msg);
00331   }
00332 
00333   if (imu_publisher_ && imu_publisher_.getNumSubscribers() > 0) {
00334     sensor_msgs::Imu imu_msg;
00335     pose_estimation_->getHeader(imu_msg.header);
00336     pose_estimation_->getOrientation(imu_msg.orientation);
00337     pose_estimation_->getImuWithBiases(imu_msg.linear_acceleration, imu_msg.angular_velocity);
00338     imu_publisher_.publish(imu_msg);
00339   }
00340 
00341   if (velocity_publisher_ && velocity_publisher_.getNumSubscribers() > 0) {
00342     geometry_msgs::Vector3Stamped velocity_msg;
00343     pose_estimation_->getVelocity(velocity_msg);
00344     velocity_publisher_.publish(velocity_msg);
00345   }
00346 
00347   if (geopose_publisher_ && geopose_publisher_.getNumSubscribers() > 0) {
00348     geographic_msgs::GeoPose geopose_msg;
00349     pose_estimation_->getGlobal(geopose_msg);
00350     geopose_publisher_.publish(geopose_msg);
00351   }
00352 
00353   if (global_fix_publisher_ && global_fix_publisher_.getNumSubscribers() > 0) {
00354     sensor_msgs::NavSatFix global_msg;
00355     pose_estimation_->getGlobal(global_msg);
00356     global_fix_publisher_.publish(global_msg);
00357   }
00358 
00359   if (euler_publisher_ && euler_publisher_.getNumSubscribers() > 0) {
00360     geometry_msgs::Vector3Stamped euler_msg;
00361     pose_estimation_->getHeader(euler_msg.header);
00362     pose_estimation_->getOrientation(euler_msg.vector.z, euler_msg.vector.y, euler_msg.vector.x);
00363     euler_publisher_.publish(euler_msg);
00364   }
00365 
00366   if ((angular_velocity_bias_publisher_ && angular_velocity_bias_publisher_.getNumSubscribers() > 0) ||
00367       (linear_acceleration_bias_publisher_ && linear_acceleration_bias_publisher_.getNumSubscribers() > 0)) {
00368     geometry_msgs::Vector3Stamped angular_velocity_msg, linear_acceleration_msg;
00369     pose_estimation_->getBias(angular_velocity_msg, linear_acceleration_msg);
00370     if (angular_velocity_bias_publisher_) angular_velocity_bias_publisher_.publish(angular_velocity_msg);
00371     if (linear_acceleration_bias_publisher_) linear_acceleration_bias_publisher_.publish(linear_acceleration_msg);
00372   }
00373 
00374   if (sensor_pose_publisher_ && sensor_pose_publisher_.getNumSubscribers() > 0) {
00375     pose_estimation_->getHeader(sensor_pose_.header);
00376     tf::Quaternion orientation;
00377     orientation.setRPY(sensor_pose_roll_, sensor_pose_pitch_, sensor_pose_yaw_);
00378     tf::quaternionTFToMsg(orientation, sensor_pose_.pose.orientation);
00379     sensor_pose_publisher_.publish(sensor_pose_);
00380   }
00381 
00382   if (getTransformBroadcaster())
00383   {
00384     transforms_.clear();
00385 
00386     pose_estimation_->getTransforms(transforms_);
00387 
00388     if (publish_world_other_transform_) {
00389       tf::StampedTransform world_to_other_transform;
00390       std::string nav_frame = pose_estimation_->parameters().getAs<std::string>("nav_frame");
00391       try {
00392         getTransformListener()->lookupTransform(nav_frame, other_frame_, ros::Time(), world_to_other_transform);
00393         pose_estimation_->updateWorldToOtherTransform(world_to_other_transform);
00394         transforms_.push_back(world_to_other_transform);
00395 
00396       } catch (tf::TransformException& e) {
00397         ROS_WARN("Could not find a transformation from %s to %s to publish the world transformation", nav_frame.c_str(), other_frame_.c_str());
00398       }
00399     }
00400 
00401     // resolve tf frames
00402     for(std::vector<tf::StampedTransform>::iterator t = transforms_.begin(); t != transforms_.end(); t++) {
00403       t->frame_id_       = tf::resolve(tf_prefix_, t->frame_id_);
00404       t->child_frame_id_ = tf::resolve(tf_prefix_, t->child_frame_id_);
00405     }
00406 
00407     getTransformBroadcaster()->sendTransform(transforms_);
00408   }
00409 
00410 #ifdef USE_HECTOR_TIMING
00411   timing_publisher_.publish(*hector_diagnostics::TimingAggregator::Instance());
00412 #endif
00413 }
00414 
00415 tf::TransformBroadcaster *PoseEstimationNode::getTransformBroadcaster() {
00416   return &transform_broadcaster_;
00417 }
00418 
00419 tf::TransformListener *PoseEstimationNode::getTransformListener() {
00420   if (!transform_listener_) transform_listener_ = new tf::TransformListener();
00421   return transform_listener_;
00422 }
00423 
00424 } // namespace hector_pose_estimation


hector_pose_estimation
Author(s): Johannes Meyer
autogenerated on Mon Aug 22 2016 03:53:32