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