00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
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_->addSystem(System::create(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
00066 pose_estimation_->parameters().initialize(ParameterRegistryROS(getPrivateNodeHandle()));
00067 getPrivateNodeHandle().param("publish_covariances", publish_covariances_, false);
00068 getPrivateNodeHandle().param("publish_world_map_transform", publish_world_other_transform_, false);
00069 getPrivateNodeHandle().param("map_frame", other_frame_, std::string());
00070
00071 if (!pose_estimation_->init()) {
00072 ROS_ERROR("Intitialization of pose estimation failed!");
00073 return false;
00074 }
00075
00076 imu_subscriber_ = getNodeHandle().subscribe("raw_imu", 10, &PoseEstimationNode::imuCallback, this);
00077 #if defined(USE_HECTOR_UAV_MSGS)
00078 baro_subscriber_ = getNodeHandle().subscribe("altimeter", 10, &PoseEstimationNode::baroCallback, this);
00079 #else
00080 height_subscriber_ = getNodeHandle().subscribe("pressure_height", 10, &PoseEstimationNode::heightCallback, this);
00081 #endif
00082 magnetic_subscriber_ = getNodeHandle().subscribe("magnetic", 10, &PoseEstimationNode::magneticCallback, this);
00083
00084 gps_subscriber_.subscribe(getNodeHandle(), "fix", 10);
00085 gps_velocity_subscriber_.subscribe(getNodeHandle(), "fix_velocity", 10);
00086 gps_synchronizer_ = new message_filters::TimeSynchronizer<sensor_msgs::NavSatFix,geometry_msgs::Vector3Stamped>(gps_subscriber_, gps_velocity_subscriber_, 10);
00087 gps_synchronizer_->registerCallback(&PoseEstimationNode::gpsCallback, this);
00088
00089 state_publisher_ = getNodeHandle().advertise<nav_msgs::Odometry>("state", 10, false);
00090 pose_publisher_ = getNodeHandle().advertise<geometry_msgs::PoseStamped>("pose", 10, false);
00091 velocity_publisher_ = getNodeHandle().advertise<geometry_msgs::Vector3Stamped>("velocity", 10, false);
00092 imu_publisher_ = getNodeHandle().advertise<sensor_msgs::Imu>("imu", 10, false);
00093 global_publisher_ = getNodeHandle().advertise<sensor_msgs::NavSatFix>("global", 10, false);
00094 euler_publisher_ = getNodeHandle().advertise<geometry_msgs::Vector3Stamped>("euler", 10, false);
00095
00096 angular_velocity_bias_publisher_ = getNodeHandle().advertise<geometry_msgs::Vector3Stamped>("angular_velocity_bias", 10, false);
00097 linear_acceleration_bias_publisher_ = getNodeHandle().advertise<geometry_msgs::Vector3Stamped>("linear_acceleration_bias", 10, false);
00098 gps_pose_publisher_ = getNodeHandle().advertise<geometry_msgs::PoseStamped>("fix/pose", 10, false);
00099
00100 poseupdate_subscriber_ = getNodeHandle().subscribe("poseupdate", 10, &PoseEstimationNode::poseupdateCallback, this);
00101 twistupdate_subscriber_ = getNodeHandle().subscribe("twistupdate", 10, &PoseEstimationNode::twistupdateCallback, this);
00102 syscommand_subscriber_ = getNodeHandle().subscribe("syscommand", 10, &PoseEstimationNode::syscommandCallback, this);
00103
00104
00105 publish();
00106
00107 return true;
00108 }
00109
00110 void PoseEstimationNode::reset() {
00111 pose_estimation_->reset();
00112 }
00113
00114 void PoseEstimationNode::cleanup() {
00115 pose_estimation_->cleanup();
00116 if (gps_synchronizer_) {
00117 delete gps_synchronizer_;
00118 gps_synchronizer_ = 0;
00119 }
00120 }
00121
00122 void PoseEstimationNode::imuCallback(const sensor_msgs::ImuConstPtr& imu) {
00123 pose_estimation_->setInput(ImuInput(*imu));
00124 pose_estimation_->update(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;
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;
00143 update.x() = magnetic->vector.x;
00144 update.y() = magnetic->vector.y;
00145 update.z() = 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, pose_estimation_->state());
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, publish_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().getAs<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 }