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/measurements/poseupdate.h>
00034 #include <hector_pose_estimation/measurements/baro.h>
00035 #include <hector_pose_estimation/measurements/height.h>
00036 #include <hector_pose_estimation/measurements/magnetic.h>
00037 #include <hector_pose_estimation/measurements/gps.h>
00038 
00039 namespace hector_pose_estimation {
00040 
00041 PoseEstimationNode::PoseEstimationNode(const SystemPtr& system)
00042   : pose_estimation_(new PoseEstimation(system))
00043   , private_nh_("~")
00044   , transform_listener_(0)
00045 {
00046   if (!system) pose_estimation_->setSystemModel(new GenericQuaternionSystemModel);
00047 
00048   pose_estimation_->addMeasurement(new PoseUpdate("poseupdate"));
00049 #if defined(USE_HECTOR_UAV_MSGS)
00050   pose_estimation_->addMeasurement(new Baro("baro"));
00051 #endif
00052   pose_estimation_->addMeasurement(new Height("height"));
00053   pose_estimation_->addMeasurement(new Magnetic("magnetic"));
00054   pose_estimation_->addMeasurement(new GPS("gps"));
00055 }
00056 
00057 PoseEstimationNode::~PoseEstimationNode()
00058 {
00059   cleanup();
00060   delete pose_estimation_;
00061   delete transform_listener_;
00062 }
00063 
00064 bool PoseEstimationNode::init() {
00065   pose_estimation_->getParameters().initialize(ParameterRegistryROS(getPrivateNodeHandle()));
00066 
00067   if (!pose_estimation_->init()) {
00068     ROS_ERROR("Intitialization of pose estimation failed!");
00069     return false;
00070   }
00071 
00072   imu_subscriber_        = getNodeHandle().subscribe("raw_imu", 10, &PoseEstimationNode::imuCallback, this);
00073 #if defined(USE_HECTOR_UAV_MSGS)
00074   baro_subscriber_       = getNodeHandle().subscribe("altimeter", 10, &PoseEstimationNode::baroCallback, this);
00075 #else
00076   height_subscriber_       = getNodeHandle().subscribe("pressure_height", 10, &PoseEstimationNode::heightCallback, this);
00077 #endif
00078   magnetic_subscriber_   = getNodeHandle().subscribe("magnetic", 10, &PoseEstimationNode::magneticCallback, this);
00079 
00080   gps_subscriber_.subscribe(getNodeHandle(), "fix", 10);
00081   gps_velocity_subscriber_.subscribe(getNodeHandle(), "fix_velocity", 10);
00082   gps_synchronizer_ = new message_filters::TimeSynchronizer<sensor_msgs::NavSatFix,geometry_msgs::Vector3Stamped>(gps_subscriber_, gps_velocity_subscriber_, 10);
00083   gps_synchronizer_->registerCallback(&PoseEstimationNode::gpsCallback, this);
00084 
00085   state_publisher_       = getNodeHandle().advertise<nav_msgs::Odometry>("state", 10, false);
00086   pose_publisher_        = getNodeHandle().advertise<geometry_msgs::PoseStamped>("pose", 10, false);
00087   velocity_publisher_    = getNodeHandle().advertise<geometry_msgs::Vector3Stamped>("velocity", 10, false);
00088   imu_publisher_         = getNodeHandle().advertise<sensor_msgs::Imu>("imu", 10, false);
00089   global_publisher_      = getNodeHandle().advertise<sensor_msgs::NavSatFix>("global", 10, false);
00090   euler_publisher_       = getNodeHandle().advertise<geometry_msgs::Vector3Stamped>("euler", 10, false);
00091 
00092   angular_velocity_bias_publisher_    = getNodeHandle().advertise<geometry_msgs::Vector3Stamped>("angular_velocity_bias", 10, false);
00093   linear_acceleration_bias_publisher_ = getNodeHandle().advertise<geometry_msgs::Vector3Stamped>("linear_acceleration_bias", 10, false);
00094   gps_pose_publisher_                 = getNodeHandle().advertise<geometry_msgs::PoseStamped>("fix/pose", 10, false);
00095 
00096   poseupdate_subscriber_  = getNodeHandle().subscribe("poseupdate", 10, &PoseEstimationNode::poseupdateCallback, this);
00097   twistupdate_subscriber_ = getNodeHandle().subscribe("twistupdate", 10, &PoseEstimationNode::twistupdateCallback, this);
00098   syscommand_subscriber_  = getNodeHandle().subscribe("syscommand", 10, &PoseEstimationNode::syscommandCallback, this);
00099 
00100   getPrivateNodeHandle().param("with_covariances", with_covariances_, false);
00101 
00102   getPrivateNodeHandle().param("publish_world_map_transform", publish_world_other_transform_, false);
00103   getPrivateNodeHandle().param("map_frame", other_frame_, std::string("map"));
00104 
00105   // publish initial state
00106   publish();
00107 
00108   return true;
00109 }
00110 
00111 void PoseEstimationNode::reset() {
00112   pose_estimation_->reset();
00113 }
00114 
00115 void PoseEstimationNode::cleanup() {
00116   pose_estimation_->cleanup();
00117   if (gps_synchronizer_) {
00118     delete gps_synchronizer_;
00119     gps_synchronizer_ = 0;
00120   }
00121 }
00122 
00123 void PoseEstimationNode::imuCallback(const sensor_msgs::ImuConstPtr& imu) {
00124   pose_estimation_->update(ImuInput(*imu), imu->header.stamp);
00125   publish();
00126 }
00127 
00128 #if defined(USE_HECTOR_UAV_MSGS)
00129 void PoseEstimationNode::baroCallback(const hector_uav_msgs::AltimeterConstPtr& altimeter) {
00130   pose_estimation_->getMeasurement("baro")->add(Baro::Update(altimeter->pressure, altimeter->qnh));
00131 }
00132 
00133 #else
00134 void PoseEstimationNode::heightCallback(const geometry_msgs::PointStampedConstPtr& height) {
00135   Height::MeasurementVector update(1);
00136   update = height->point.z;
00137   pose_estimation_->getMeasurement("height")->add(Height::Update(update));
00138 }
00139 #endif
00140 
00141 void PoseEstimationNode::magneticCallback(const geometry_msgs::Vector3StampedConstPtr& magnetic) {
00142   Magnetic::MeasurementVector update(3);
00143   update(1) = magnetic->vector.x;
00144   update(2) = magnetic->vector.y;
00145   update(3) = magnetic->vector.z;
00146   pose_estimation_->getMeasurement("magnetic")->add(Magnetic::Update(update));
00147 }
00148 
00149 void PoseEstimationNode::gpsCallback(const sensor_msgs::NavSatFixConstPtr& gps, const geometry_msgs::Vector3StampedConstPtr& gps_velocity) {
00150   if (gps->status.status == sensor_msgs::NavSatStatus::STATUS_NO_FIX) return;
00151   GPS::Update update;
00152   update.latitude = gps->latitude * M_PI/180.0;
00153   update.longitude = gps->longitude * M_PI/180.0;
00154   update.velocity_north =  gps_velocity->vector.x;
00155   update.velocity_east  = -gps_velocity->vector.y;
00156   pose_estimation_->getMeasurement("gps")->add(update);
00157 
00158   if (gps_pose_publisher_.getNumSubscribers() > 0) {
00159     geometry_msgs::PoseStamped gps_pose;
00160     pose_estimation_->getHeader(gps_pose.header);
00161     gps_pose.header.seq = gps->header.seq;
00162     gps_pose.header.stamp = gps->header.stamp;
00163     GPSModel::MeasurementVector y = boost::shared_static_cast<GPS>(pose_estimation_->getMeasurement("gps"))->getVector(update);
00164     gps_pose.pose.position.x = y(1);
00165     gps_pose.pose.position.y = y(2);
00166     gps_pose.pose.position.z = gps->altitude - pose_estimation_->globalReference()->position().altitude;
00167     double track = atan2(gps_velocity->vector.y, gps_velocity->vector.x);
00168     gps_pose.pose.orientation.w = cos(track/2);
00169     gps_pose.pose.orientation.z = sin(track/2);
00170     gps_pose_publisher_.publish(gps_pose);
00171   }
00172 }
00173 
00174 void PoseEstimationNode::poseupdateCallback(const geometry_msgs::PoseWithCovarianceStampedConstPtr& pose) {
00175   pose_estimation_->getMeasurement("poseupdate")->add(PoseUpdate::Update(pose));
00176 }
00177 
00178 void PoseEstimationNode::twistupdateCallback(const geometry_msgs::TwistWithCovarianceStampedConstPtr& twist) {
00179   pose_estimation_->getMeasurement("poseupdate")->add(PoseUpdate::Update(twist));
00180 }
00181 
00182 void PoseEstimationNode::syscommandCallback(const std_msgs::StringConstPtr& syscommand) {
00183   if (syscommand->data == "reset") {
00184     ROS_INFO("Resetting pose_estimation");
00185     pose_estimation_->reset();
00186     publish();
00187   }
00188 }
00189 
00190 void PoseEstimationNode::publish() {
00191   if (state_publisher_) {
00192     nav_msgs::Odometry state;
00193     pose_estimation_->getState(state, with_covariances_);
00194     state_publisher_.publish(state);
00195   }
00196 
00197   if (pose_publisher_) {
00198     geometry_msgs::PoseStamped pose_msg;
00199     pose_estimation_->getPose(pose_msg);
00200     pose_publisher_.publish(pose_msg);
00201   }
00202 
00203   if (imu_publisher_) {
00204     sensor_msgs::Imu imu_msg;
00205     pose_estimation_->getHeader(imu_msg.header);
00206     pose_estimation_->getOrientation(imu_msg.orientation);
00207     pose_estimation_->getImuWithBiases(imu_msg.linear_acceleration, imu_msg.angular_velocity);
00208     imu_publisher_.publish(imu_msg);
00209   }
00210 
00211   if (velocity_publisher_) {
00212     geometry_msgs::Vector3Stamped velocity_msg;
00213     pose_estimation_->getVelocity(velocity_msg);
00214     velocity_publisher_.publish(velocity_msg);
00215   }
00216 
00217   if (global_publisher_) {
00218     sensor_msgs::NavSatFix global_msg;
00219     pose_estimation_->getGlobalPosition(global_msg);
00220     global_publisher_.publish(global_msg);
00221   }
00222 
00223   if (euler_publisher_) {
00224     geometry_msgs::Vector3Stamped euler_msg;
00225     pose_estimation_->getHeader(euler_msg.header);
00226     pose_estimation_->getOrientation(euler_msg.vector.z, euler_msg.vector.y, euler_msg.vector.x);
00227     euler_publisher_.publish(euler_msg);
00228   }
00229 
00230   if (angular_velocity_bias_publisher_ || linear_acceleration_bias_publisher_) {
00231     geometry_msgs::Vector3Stamped angular_velocity_msg, linear_acceleration_msg;
00232     pose_estimation_->getBias(angular_velocity_msg, linear_acceleration_msg);
00233     if (angular_velocity_bias_publisher_) angular_velocity_bias_publisher_.publish(angular_velocity_msg);
00234     if (linear_acceleration_bias_publisher_) linear_acceleration_bias_publisher_.publish(linear_acceleration_msg);
00235   }
00236 
00237   if (getTransformBroadcaster())
00238   {
00239     transforms_.clear();
00240 
00241     pose_estimation_->getTransforms(transforms_);
00242 
00243     if (publish_world_other_transform_) {
00244       tf::StampedTransform world_to_other_transform;
00245       std::string nav_frame = pose_estimation_->parameters().get<std::string>("nav_frame");
00246       try {
00247         getTransformListener()->lookupTransform(nav_frame, other_frame_, ros::Time(), world_to_other_transform);
00248         pose_estimation_->updateWorldToOtherTransform(world_to_other_transform);
00249         transforms_.push_back(world_to_other_transform);
00250 
00251       } catch (tf::TransformException& e) {
00252         ROS_WARN("Could not find a transformation from %s to %s to publish the world transformation", nav_frame.c_str(), other_frame_.c_str());
00253       }
00254     }
00255 
00256     getTransformBroadcaster()->sendTransform(transforms_);
00257   }
00258 }
00259 
00260 tf::TransformBroadcaster *PoseEstimationNode::getTransformBroadcaster() {
00261   return &transform_broadcaster_;
00262 }
00263 
00264 tf::TransformListener *PoseEstimationNode::getTransformListener() {
00265   if (!transform_listener_) transform_listener_ = new tf::TransformListener();
00266   return transform_listener_;
00267 }
00268 
00269 } // namespace hector_pose_estimation
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Properties Friends


hector_pose_estimation
Author(s): Johannes Meyer
autogenerated on Mon Jul 15 2013 16:51:16