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
00030
00031
00032
00033 #include "robot_localization/navsat_transform.h"
00034 #include "robot_localization/filter_common.h"
00035 #include "robot_localization/filter_utilities.h"
00036 #include "robot_localization/navsat_conversions.h"
00037 #include "robot_localization/ros_filter_utilities.h"
00038
00039 #include <tf2_geometry_msgs/tf2_geometry_msgs.h>
00040
00041 #include <XmlRpcException.h>
00042
00043 #include <string>
00044
00045 namespace RobotLocalization
00046 {
00047 NavSatTransform::NavSatTransform() :
00048 magnetic_declination_(0.0),
00049 utm_odom_tf_yaw_(0.0),
00050 yaw_offset_(0.0),
00051 transform_timeout_(ros::Duration(0)),
00052 broadcast_utm_transform_(false),
00053 has_transform_odom_(false),
00054 has_transform_gps_(false),
00055 has_transform_imu_(false),
00056 transform_good_(false),
00057 gps_frame_id_(""),
00058 gps_updated_(false),
00059 odom_updated_(false),
00060 publish_gps_(false),
00061 use_odometry_yaw_(false),
00062 use_manual_datum_(false),
00063 zero_altitude_(false),
00064 world_frame_id_("odom"),
00065 base_link_frame_id_("base_link"),
00066 utm_zone_(""),
00067 tf_listener_(tf_buffer_)
00068 {
00069 latest_utm_covariance_.resize(POSE_SIZE, POSE_SIZE);
00070 latest_odom_covariance_.resize(POSE_SIZE, POSE_SIZE);
00071 }
00072
00073 NavSatTransform::~NavSatTransform()
00074 {
00075 }
00076
00077 void NavSatTransform::run()
00078 {
00079 ros::Time::init();
00080
00081 double frequency = 10.0;
00082 double delay = 0.0;
00083 double transform_timeout = 0.0;
00084
00085 ros::NodeHandle nh;
00086 ros::NodeHandle nh_priv("~");
00087
00088
00089 nh_priv.getParam("magnetic_declination_radians", magnetic_declination_);
00090 nh_priv.param("yaw_offset", yaw_offset_, 0.0);
00091 nh_priv.param("broadcast_utm_transform", broadcast_utm_transform_, false);
00092 nh_priv.param("zero_altitude", zero_altitude_, false);
00093 nh_priv.param("publish_filtered_gps", publish_gps_, false);
00094 nh_priv.param("use_odometry_yaw", use_odometry_yaw_, false);
00095 nh_priv.param("wait_for_datum", use_manual_datum_, false);
00096 nh_priv.param("frequency", frequency, 10.0);
00097 nh_priv.param("delay", delay, 0.0);
00098 nh_priv.param("transform_timeout", transform_timeout, 0.0);
00099 transform_timeout_.fromSec(transform_timeout);
00100
00101
00102 ros::ServiceServer datum_srv = nh.advertiseService("datum", &NavSatTransform::datumCallback, this);
00103
00104 if (use_manual_datum_ && nh_priv.hasParam("datum"))
00105 {
00106 XmlRpc::XmlRpcValue datum_config;
00107
00108 try
00109 {
00110 double datum_lat;
00111 double datum_lon;
00112 double datum_yaw;
00113
00114 nh_priv.getParam("datum", datum_config);
00115
00116
00117
00118 ROS_ASSERT(datum_config.getType() == XmlRpc::XmlRpcValue::TypeArray);
00119 ROS_ASSERT(datum_config.size() >= 3);
00120
00121 if (datum_config.size() > 3)
00122 {
00123 ROS_WARN_STREAM("Deprecated datum parameter configuration detected. Only the first three parameters "
00124 "(latitude, longitude, yaw) will be used. frame_ids will be derived from odometry and navsat inputs.");
00125 }
00126
00127 std::ostringstream ostr;
00128 ostr << datum_config[0] << " " << datum_config[1] << " " << datum_config[2];
00129 std::istringstream istr(ostr.str());
00130 istr >> datum_lat >> datum_lon >> datum_yaw;
00131
00132
00133 std::string tf_prefix = "";
00134 std::string tf_prefix_path = "";
00135 if (nh_priv.searchParam("tf_prefix", tf_prefix_path))
00136 {
00137 nh_priv.getParam(tf_prefix_path, tf_prefix);
00138 }
00139
00140
00141 FilterUtilities::appendPrefix(tf_prefix, world_frame_id_);
00142 FilterUtilities::appendPrefix(tf_prefix, base_link_frame_id_);
00143
00144 robot_localization::SetDatum::Request request;
00145 request.geo_pose.position.latitude = datum_lat;
00146 request.geo_pose.position.longitude = datum_lon;
00147 request.geo_pose.position.altitude = 0.0;
00148 tf2::Quaternion quat;
00149 quat.setRPY(0.0, 0.0, datum_yaw);
00150 request.geo_pose.orientation = tf2::toMsg(quat);
00151 robot_localization::SetDatum::Response response;
00152 datumCallback(request, response);
00153 }
00154 catch (XmlRpc::XmlRpcException &e)
00155 {
00156 ROS_ERROR_STREAM("ERROR reading sensor config: " << e.getMessage() <<
00157 " for process_noise_covariance (type: " << datum_config.getType() << ")");
00158 }
00159 }
00160
00161 ros::Subscriber odom_sub = nh.subscribe("odometry/filtered", 1, &NavSatTransform::odomCallback, this);
00162 ros::Subscriber gps_sub = nh.subscribe("gps/fix", 1, &NavSatTransform::gpsFixCallback, this);
00163 ros::Subscriber imu_sub;
00164
00165 if (!use_odometry_yaw_ && !use_manual_datum_)
00166 {
00167 imu_sub = nh.subscribe("imu/data", 1, &NavSatTransform::imuCallback, this);
00168 }
00169
00170 ros::Publisher gps_odom_pub = nh.advertise<nav_msgs::Odometry>("odometry/gps", 10);
00171 ros::Publisher filtered_gps_pub;
00172
00173 if (publish_gps_)
00174 {
00175 filtered_gps_pub = nh.advertise<sensor_msgs::NavSatFix>("gps/filtered", 10);
00176 }
00177
00178
00179
00180 ros::Duration start_delay(delay);
00181 start_delay.sleep();
00182
00183 ros::Rate rate(frequency);
00184 while (ros::ok())
00185 {
00186 ros::spinOnce();
00187
00188 if (!transform_good_)
00189 {
00190 computeTransform();
00191
00192 if (transform_good_ && !use_odometry_yaw_ && !use_manual_datum_)
00193 {
00194
00195 imu_sub.shutdown();
00196 }
00197 }
00198 else
00199 {
00200 nav_msgs::Odometry gps_odom;
00201 if (prepareGpsOdometry(gps_odom))
00202 {
00203 gps_odom_pub.publish(gps_odom);
00204 }
00205
00206 if (publish_gps_)
00207 {
00208 sensor_msgs::NavSatFix odom_gps;
00209 if (prepareFilteredGps(odom_gps))
00210 {
00211 filtered_gps_pub.publish(odom_gps);
00212 }
00213 }
00214 }
00215
00216 rate.sleep();
00217 }
00218 }
00219
00220 void NavSatTransform::computeTransform()
00221 {
00222
00223
00224
00225 if (!transform_good_ &&
00226 has_transform_odom_ &&
00227 has_transform_gps_ &&
00228 has_transform_imu_)
00229 {
00230
00231
00232 tf2::Transform transform_utm_pose_corrected;
00233 if (!use_manual_datum_)
00234 {
00235 getRobotOriginUtmPose(transform_utm_pose_, transform_utm_pose_corrected, ros::Time(0));
00236 }
00237 else
00238 {
00239 transform_utm_pose_corrected = transform_utm_pose_;
00240 }
00241
00242
00243 tf2::Matrix3x3 mat(transform_orientation_);
00244
00245
00246 double imu_roll;
00247 double imu_pitch;
00248 double imu_yaw;
00249 mat.getRPY(imu_roll, imu_pitch, imu_yaw);
00250
00251
00252
00253
00254
00255
00256
00257
00258
00259
00260
00261
00262
00263
00264
00265
00266
00267
00268 imu_yaw += (magnetic_declination_ + yaw_offset_);
00269
00270 ROS_INFO_STREAM("Corrected for magnetic declination of " << std::fixed << magnetic_declination_ <<
00271 " and user-specified offset of " << yaw_offset_ << ". Transform heading factor is now " << imu_yaw);
00272
00273
00274 tf2::Quaternion imu_quat;
00275 imu_quat.setRPY(0.0, 0.0, imu_yaw);
00276
00277
00278
00279
00280 tf2::Transform utm_pose_with_orientation;
00281 utm_pose_with_orientation.setOrigin(transform_utm_pose_corrected.getOrigin());
00282 utm_pose_with_orientation.setRotation(imu_quat);
00283
00284 utm_world_transform_.mult(transform_world_pose_, utm_pose_with_orientation.inverse());
00285
00286 utm_world_trans_inverse_ = utm_world_transform_.inverse();
00287
00288 ROS_INFO_STREAM("Transform world frame pose is: " << transform_world_pose_);
00289 ROS_INFO_STREAM("World frame->utm transform is " << utm_world_transform_);
00290
00291 transform_good_ = true;
00292
00293
00294 if (broadcast_utm_transform_)
00295 {
00296 geometry_msgs::TransformStamped utm_transform_stamped;
00297 utm_transform_stamped.header.stamp = ros::Time::now();
00298 utm_transform_stamped.header.frame_id = world_frame_id_;
00299 utm_transform_stamped.child_frame_id = "utm";
00300 utm_transform_stamped.transform = tf2::toMsg(utm_world_transform_);
00301 utm_transform_stamped.transform.translation.z = (zero_altitude_ ? 0.0 : utm_transform_stamped.transform.translation.z);
00302 utm_broadcaster_.sendTransform(utm_transform_stamped);
00303 }
00304 }
00305 }
00306
00307 bool NavSatTransform::datumCallback(robot_localization::SetDatum::Request& request,
00308 robot_localization::SetDatum::Response&)
00309 {
00310
00311
00312
00313 use_manual_datum_ = true;
00314
00315 transform_good_ = false;
00316
00317 sensor_msgs::NavSatFix *fix = new sensor_msgs::NavSatFix();
00318 fix->latitude = request.geo_pose.position.latitude;
00319 fix->longitude = request.geo_pose.position.longitude;
00320 fix->altitude = request.geo_pose.position.altitude;
00321 fix->header.stamp = ros::Time::now();
00322 fix->position_covariance[0] = 0.1;
00323 fix->position_covariance[4] = 0.1;
00324 fix->position_covariance[8] = 0.1;
00325 fix->position_covariance_type = sensor_msgs::NavSatStatus::STATUS_FIX;
00326 sensor_msgs::NavSatFixConstPtr fix_ptr(fix);
00327 setTransformGps(fix_ptr);
00328
00329 nav_msgs::Odometry *odom = new nav_msgs::Odometry();
00330 odom->pose.pose.orientation.x = 0;
00331 odom->pose.pose.orientation.y = 0;
00332 odom->pose.pose.orientation.z = 0;
00333 odom->pose.pose.orientation.w = 1;
00334 odom->pose.pose.position.x = 0;
00335 odom->pose.pose.position.y = 0;
00336 odom->pose.pose.position.z = 0;
00337 odom->header.frame_id = world_frame_id_;
00338 odom->child_frame_id = base_link_frame_id_;
00339 nav_msgs::OdometryConstPtr odom_ptr(odom);
00340 setTransformOdometry(odom_ptr);
00341
00342 sensor_msgs::Imu *imu = new sensor_msgs::Imu();
00343 imu->orientation = request.geo_pose.orientation;
00344 imu->header.frame_id = base_link_frame_id_;
00345 sensor_msgs::ImuConstPtr imu_ptr(imu);
00346 imuCallback(imu_ptr);
00347
00348 return true;
00349 }
00350
00351 void NavSatTransform::getRobotOriginUtmPose(const tf2::Transform &gps_utm_pose,
00352 tf2::Transform &robot_utm_pose,
00353 const ros::Time &transform_time)
00354 {
00355 robot_utm_pose.setIdentity();
00356
00357
00358 tf2::Transform offset;
00359 bool can_transform = RosFilterUtilities::lookupTransformSafe(tf_buffer_,
00360 base_link_frame_id_,
00361 gps_frame_id_,
00362 transform_time,
00363 ros::Duration(transform_timeout_),
00364 offset);
00365
00366 if (can_transform)
00367 {
00368
00369 tf2::Quaternion utm_orientation = transform_orientation_;
00370 tf2::Matrix3x3 mat(utm_orientation);
00371
00372
00373 double roll;
00374 double pitch;
00375 double yaw;
00376 mat.getRPY(roll, pitch, yaw);
00377 yaw += (magnetic_declination_ + yaw_offset_);
00378 utm_orientation.setRPY(roll, pitch, yaw);
00379
00380
00381
00382
00383 offset.setOrigin(tf2::quatRotate(utm_orientation, offset.getOrigin()));
00384 offset.setRotation(tf2::Quaternion::getIdentity());
00385
00386
00387 robot_utm_pose = offset.inverse() * gps_utm_pose;
00388 }
00389 else
00390 {
00391 if (gps_frame_id_ != "")
00392 {
00393 ROS_WARN_STREAM_ONCE("Unable to obtain " << base_link_frame_id_ << "->" << gps_frame_id_ <<
00394 " transform. Will assume navsat device is mounted at robot's origin");
00395 }
00396
00397 robot_utm_pose = gps_utm_pose;
00398 }
00399 }
00400
00401 void NavSatTransform::getRobotOriginWorldPose(const tf2::Transform &gps_odom_pose,
00402 tf2::Transform &robot_odom_pose,
00403 const ros::Time &transform_time)
00404 {
00405 robot_odom_pose.setIdentity();
00406
00407
00408 tf2::Transform gps_offset_rotated;
00409 bool can_transform = RosFilterUtilities::lookupTransformSafe(tf_buffer_,
00410 base_link_frame_id_,
00411 gps_frame_id_,
00412 transform_time,
00413 transform_timeout_,
00414 gps_offset_rotated);
00415
00416 if (can_transform)
00417 {
00418 tf2::Transform robot_orientation;
00419 can_transform = RosFilterUtilities::lookupTransformSafe(tf_buffer_,
00420 world_frame_id_,
00421 base_link_frame_id_,
00422 transform_time,
00423 transform_timeout_,
00424 robot_orientation);
00425
00426 if (can_transform)
00427 {
00428 gps_offset_rotated.setOrigin(tf2::quatRotate(robot_orientation.getRotation(), gps_offset_rotated.getOrigin()));
00429 robot_odom_pose = gps_offset_rotated.inverse() * gps_odom_pose;
00430 }
00431 else
00432 {
00433 ROS_WARN_STREAM_THROTTLE(5.0, "Could not obtain " << world_frame_id_ << "->" << base_link_frame_id_ <<
00434 " transform. Will not remove offset of navsat device from robot's origin.");
00435 }
00436 }
00437 else
00438 {
00439 ROS_WARN_STREAM_THROTTLE(5.0, "Could not obtain " << base_link_frame_id_ << "->" << gps_frame_id_ <<
00440 " transform. Will not remove offset of navsat device from robot's origin.");
00441 }
00442 }
00443
00444 void NavSatTransform::gpsFixCallback(const sensor_msgs::NavSatFixConstPtr& msg)
00445 {
00446 gps_frame_id_ = msg->header.frame_id;
00447
00448 if (gps_frame_id_.empty())
00449 {
00450 ROS_WARN_STREAM_ONCE("NavSatFix message has empty frame_id. Will assume navsat device is mounted at robot's "
00451 "origin.");
00452 }
00453
00454
00455 bool good_gps = (msg->status.status != sensor_msgs::NavSatStatus::STATUS_NO_FIX &&
00456 !std::isnan(msg->altitude) &&
00457 !std::isnan(msg->latitude) &&
00458 !std::isnan(msg->longitude));
00459
00460 if (good_gps)
00461 {
00462
00463
00464 if (!transform_good_ && !use_manual_datum_)
00465 {
00466 setTransformGps(msg);
00467 }
00468
00469 double utmX = 0;
00470 double utmY = 0;
00471 std::string utm_zone_tmp;
00472 NavsatConversions::LLtoUTM(msg->latitude, msg->longitude, utmY, utmX, utm_zone_tmp);
00473 latest_utm_pose_.setOrigin(tf2::Vector3(utmX, utmY, msg->altitude));
00474 latest_utm_covariance_.setZero();
00475
00476
00477 for (size_t i = 0; i < POSITION_SIZE; i++)
00478 {
00479 for (size_t j = 0; j < POSITION_SIZE; j++)
00480 {
00481 latest_utm_covariance_(i, j) = msg->position_covariance[POSITION_SIZE * i + j];
00482 }
00483 }
00484
00485 gps_update_time_ = msg->header.stamp;
00486 gps_updated_ = true;
00487 }
00488 }
00489
00490 void NavSatTransform::imuCallback(const sensor_msgs::ImuConstPtr& msg)
00491 {
00492
00493
00494 if (has_transform_odom_)
00495 {
00496
00497
00498
00499
00500 tf2::fromMsg(msg->orientation, transform_orientation_);
00501
00502
00503 tf2::Transform target_frame_trans;
00504 bool can_transform = RosFilterUtilities::lookupTransformSafe(tf_buffer_,
00505 base_link_frame_id_,
00506 msg->header.frame_id,
00507 msg->header.stamp,
00508 transform_timeout_,
00509 target_frame_trans);
00510
00511 if (can_transform)
00512 {
00513 double roll_offset = 0;
00514 double pitch_offset = 0;
00515 double yaw_offset = 0;
00516 double roll = 0;
00517 double pitch = 0;
00518 double yaw = 0;
00519 RosFilterUtilities::quatToRPY(target_frame_trans.getRotation(), roll_offset, pitch_offset, yaw_offset);
00520 RosFilterUtilities::quatToRPY(transform_orientation_, roll, pitch, yaw);
00521
00522 ROS_DEBUG_STREAM("Initial orientation is " << transform_orientation_);
00523
00524
00525 tf2::Vector3 rpy_angles(FilterUtilities::clampRotation(roll - roll_offset),
00526 FilterUtilities::clampRotation(pitch - pitch_offset),
00527 FilterUtilities::clampRotation(yaw - yaw_offset));
00528
00529
00530
00531
00532 tf2::Matrix3x3 mat;
00533 mat.setRPY(0.0, 0.0, yaw_offset);
00534 rpy_angles = mat * rpy_angles;
00535 transform_orientation_.setRPY(rpy_angles.getX(), rpy_angles.getY(), rpy_angles.getZ());
00536
00537 ROS_DEBUG_STREAM("Initial corrected orientation roll, pitch, yaw is (" <<
00538 rpy_angles.getX() << ", " << rpy_angles.getY() << ", " << rpy_angles.getZ() << ")");
00539
00540 has_transform_imu_ = true;
00541 }
00542 }
00543 }
00544
00545 void NavSatTransform::odomCallback(const nav_msgs::OdometryConstPtr& msg)
00546 {
00547 world_frame_id_ = msg->header.frame_id;
00548 base_link_frame_id_ = msg->child_frame_id;
00549
00550 if (!transform_good_ && !use_manual_datum_)
00551 {
00552 setTransformOdometry(msg);
00553 }
00554
00555 tf2::fromMsg(msg->pose.pose, latest_world_pose_);
00556 latest_odom_covariance_.setZero();
00557 for(size_t row = 0; row < POSE_SIZE; ++row)
00558 {
00559 for(size_t col = 0; col < POSE_SIZE; ++col)
00560 {
00561 latest_odom_covariance_(row, col) = msg->pose.covariance[row * POSE_SIZE + col];
00562 }
00563 }
00564
00565 odom_update_time_ = msg->header.stamp;
00566 odom_updated_ = true;
00567 }
00568
00569
00570 bool NavSatTransform::prepareFilteredGps(sensor_msgs::NavSatFix &filtered_gps)
00571 {
00572 bool new_data = false;
00573
00574 if (transform_good_ && odom_updated_)
00575 {
00576 tf2::Transform odom_as_utm;
00577
00578 odom_as_utm.mult(utm_world_trans_inverse_, latest_world_pose_);
00579 odom_as_utm.setRotation(tf2::Quaternion::getIdentity());
00580
00581
00582 tf2::Matrix3x3 rot(utm_world_trans_inverse_.getRotation());
00583 Eigen::MatrixXd rot_6d(POSE_SIZE, POSE_SIZE);
00584 rot_6d.setIdentity();
00585
00586 for (size_t rInd = 0; rInd < POSITION_SIZE; ++rInd)
00587 {
00588 rot_6d(rInd, 0) = rot.getRow(rInd).getX();
00589 rot_6d(rInd, 1) = rot.getRow(rInd).getY();
00590 rot_6d(rInd, 2) = rot.getRow(rInd).getZ();
00591 rot_6d(rInd+POSITION_SIZE, 3) = rot.getRow(rInd).getX();
00592 rot_6d(rInd+POSITION_SIZE, 4) = rot.getRow(rInd).getY();
00593 rot_6d(rInd+POSITION_SIZE, 5) = rot.getRow(rInd).getZ();
00594 }
00595
00596
00597 latest_odom_covariance_ = rot_6d * latest_odom_covariance_.eval() * rot_6d.transpose();
00598
00599
00600 NavsatConversions::UTMtoLL(odom_as_utm.getOrigin().getY(),
00601 odom_as_utm.getOrigin().getX(),
00602 utm_zone_,
00603 filtered_gps.latitude,
00604 filtered_gps.longitude);
00605 filtered_gps.altitude = odom_as_utm.getOrigin().getZ();
00606
00607
00608 for (size_t i = 0; i < POSITION_SIZE; i++)
00609 {
00610 for (size_t j = 0; j < POSITION_SIZE; j++)
00611 {
00612 filtered_gps.position_covariance[POSITION_SIZE * i + j] = latest_odom_covariance_(i, j);
00613 }
00614 }
00615
00616 filtered_gps.position_covariance_type = sensor_msgs::NavSatFix::COVARIANCE_TYPE_KNOWN;
00617 filtered_gps.status.status = sensor_msgs::NavSatStatus::STATUS_GBAS_FIX;
00618 filtered_gps.header.frame_id = "gps";
00619 filtered_gps.header.stamp = odom_update_time_;
00620
00621
00622 odom_updated_ = false;
00623 new_data = true;
00624 }
00625
00626 return new_data;
00627 }
00628
00629 bool NavSatTransform::prepareGpsOdometry(nav_msgs::Odometry &gps_odom)
00630 {
00631 bool new_data = false;
00632
00633 if (transform_good_ && gps_updated_ && odom_updated_)
00634 {
00635 tf2::Transform transformed_utm_gps;
00636
00637 transformed_utm_gps.mult(utm_world_transform_, latest_utm_pose_);
00638 transformed_utm_gps.setRotation(tf2::Quaternion::getIdentity());
00639
00640
00641 gps_odom.header.frame_id = world_frame_id_;
00642 gps_odom.header.stamp = gps_update_time_;
00643
00644
00645 tf2::Transform transformed_utm_robot;
00646 getRobotOriginWorldPose(transformed_utm_gps, transformed_utm_robot, gps_odom.header.stamp);
00647
00648
00649 tf2::Matrix3x3 rot(utm_world_transform_.getRotation());
00650 Eigen::MatrixXd rot_6d(POSE_SIZE, POSE_SIZE);
00651 rot_6d.setIdentity();
00652
00653 for (size_t rInd = 0; rInd < POSITION_SIZE; ++rInd)
00654 {
00655 rot_6d(rInd, 0) = rot.getRow(rInd).getX();
00656 rot_6d(rInd, 1) = rot.getRow(rInd).getY();
00657 rot_6d(rInd, 2) = rot.getRow(rInd).getZ();
00658 rot_6d(rInd+POSITION_SIZE, 3) = rot.getRow(rInd).getX();
00659 rot_6d(rInd+POSITION_SIZE, 4) = rot.getRow(rInd).getY();
00660 rot_6d(rInd+POSITION_SIZE, 5) = rot.getRow(rInd).getZ();
00661 }
00662
00663
00664 latest_utm_covariance_ = rot_6d * latest_utm_covariance_.eval() * rot_6d.transpose();
00665
00666
00667 tf2::toMsg(transformed_utm_robot, gps_odom.pose.pose);
00668 gps_odom.pose.pose.position.z = (zero_altitude_ ? 0.0 : gps_odom.pose.pose.position.z);
00669
00670
00671 for (size_t i = 0; i < POSE_SIZE; i++)
00672 {
00673 for (size_t j = 0; j < POSE_SIZE; j++)
00674 {
00675 gps_odom.pose.covariance[POSE_SIZE * i + j] = latest_utm_covariance_(i, j);
00676 }
00677 }
00678
00679
00680 gps_updated_ = false;
00681 new_data = true;
00682 }
00683
00684 return new_data;
00685 }
00686
00687 void NavSatTransform::setTransformGps(const sensor_msgs::NavSatFixConstPtr& msg)
00688 {
00689 double utm_x = 0;
00690 double utm_y = 0;
00691 NavsatConversions::LLtoUTM(msg->latitude, msg->longitude, utm_y, utm_x, utm_zone_);
00692
00693 ROS_INFO_STREAM("Datum (latitude, longitude, altitude) is (" << std::fixed << msg->latitude << ", " <<
00694 msg->longitude << ", " << msg->altitude << ")");
00695 ROS_INFO_STREAM("Datum UTM coordinate is (" << std::fixed << utm_x << ", " << utm_y << ")");
00696
00697 transform_utm_pose_.setOrigin(tf2::Vector3(utm_x, utm_y, msg->altitude));
00698 transform_utm_pose_.setRotation(tf2::Quaternion::getIdentity());
00699 has_transform_gps_ = true;
00700 }
00701
00702 void NavSatTransform::setTransformOdometry(const nav_msgs::OdometryConstPtr& msg)
00703 {
00704 tf2::fromMsg(msg->pose.pose, transform_world_pose_);
00705 has_transform_odom_ = true;
00706
00707 ROS_INFO_STREAM("Initial odometry pose is " << transform_world_pose_);
00708
00709
00710
00711
00712
00713 if (!transform_good_ && use_odometry_yaw_ && !use_manual_datum_)
00714 {
00715 sensor_msgs::Imu *imu = new sensor_msgs::Imu();
00716 imu->orientation = msg->pose.pose.orientation;
00717 imu->header.frame_id = msg->child_frame_id;
00718 imu->header.stamp = msg->header.stamp;
00719 sensor_msgs::ImuConstPtr imuPtr(imu);
00720 imuCallback(imuPtr);
00721 }
00722 }
00723
00724 }