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