47 magnetic_declination_(0.0),
48 utm_odom_tf_yaw_(0.0),
50 transform_timeout_(
ros::Duration(0)),
51 broadcast_utm_transform_(false),
52 broadcast_utm_transform_as_parent_frame_(false),
53 has_transform_odom_(false),
54 has_transform_gps_(false),
55 has_transform_imu_(false),
56 transform_good_(false),
61 use_odometry_yaw_(false),
62 use_manual_datum_(false),
63 world_frame_param_loaded_(false),
64 base_link_frame_param_loaded_(false),
65 zero_altitude_(false),
66 world_frame_id_(
"odom"),
67 base_link_frame_id_(
"base_link"),
69 tf_listener_(tf_buffer_)
81 ROS_INFO(
"Waiting for valid clock time...");
83 ROS_INFO(
"Valid clock time received. Starting node.");
85 double frequency = 10.0;
87 double transform_timeout = 0.0;
101 nh_priv.
param(
"frequency", frequency, 10.0);
102 nh_priv.
param(
"delay", delay, 0.0);
103 nh_priv.
param(
"transform_timeout", transform_timeout, 0.0);
109 if (nh_priv.
hasParam(
"world_frame"))
114 if (nh_priv.
hasParam(
"base_link_frame"))
122 if (nh_priv.
hasParam(
"world_frame"))
125 "This parameter is only used if parameter 'wait_for_datum' is true.");
127 if (nh_priv.
hasParam(
"base_link_frame"))
129 ROS_WARN_STREAM(
"Parameter 'base_link_frame' is set and will be unused." 130 "This parameter is only used if parameter 'wait_for_datum' is true.");
147 nh_priv.
getParam(
"datum", datum_config);
154 if (datum_config.
size() > 3)
156 ROS_WARN_STREAM(
"Deprecated datum parameter configuration detected. Only the first three parameters " 157 "(latitude, longitude, yaw) will be used. frame_ids will be derived from odometry and navsat inputs.");
160 std::ostringstream ostr;
161 ostr << std::setprecision(20) << datum_config[0] <<
" " << datum_config[1] <<
" " << datum_config[2];
162 std::istringstream istr(ostr.str());
163 istr >> datum_lat >> datum_lon >> datum_yaw;
166 std::string tf_prefix =
"";
167 std::string tf_prefix_path =
"";
168 if (nh_priv.
searchParam(
"tf_prefix", tf_prefix_path))
170 nh_priv.
getParam(tf_prefix_path, tf_prefix);
177 robot_localization::SetDatum::Request request;
178 request.geo_pose.position.latitude = datum_lat;
179 request.geo_pose.position.longitude = datum_lon;
180 request.geo_pose.position.altitude = 0.0;
182 quat.
setRPY(0.0, 0.0, datum_yaw);
183 request.geo_pose.orientation =
tf2::toMsg(quat);
184 robot_localization::SetDatum::Response response;
190 " for process_noise_covariance (type: " << datum_config.
getType() <<
")");
208 filtered_gps_pub = nh.
advertise<sensor_msgs::NavSatFix>(
"gps/filtered", 10);
233 nav_msgs::Odometry gps_odom;
236 gps_odom_pub.
publish(gps_odom);
241 sensor_msgs::NavSatFix odom_gps;
244 filtered_gps_pub.publish(odom_gps);
282 mat.
getRPY(imu_roll, imu_pitch, imu_yaw);
308 " Transform heading factor is now " << imu_yaw);
312 imu_quat.
setRPY(0.0, 0.0, imu_yaw);
333 geometry_msgs::TransformStamped utm_transform_stamped;
340 0.0 : utm_transform_stamped.transform.translation.z);
347 robot_localization::SetDatum::Response&)
356 sensor_msgs::NavSatFix *fix =
new sensor_msgs::NavSatFix();
357 fix->latitude = request.geo_pose.position.latitude;
358 fix->longitude = request.geo_pose.position.longitude;
359 fix->altitude = request.geo_pose.position.altitude;
361 fix->position_covariance[0] = 0.1;
362 fix->position_covariance[4] = 0.1;
363 fix->position_covariance[8] = 0.1;
364 fix->position_covariance_type = sensor_msgs::NavSatStatus::STATUS_FIX;
365 sensor_msgs::NavSatFixConstPtr fix_ptr(fix);
368 nav_msgs::Odometry *odom =
new nav_msgs::Odometry();
369 odom->pose.pose.orientation.x = 0;
370 odom->pose.pose.orientation.y = 0;
371 odom->pose.pose.orientation.z = 0;
372 odom->pose.pose.orientation.w = 1;
373 odom->pose.pose.position.x = 0;
374 odom->pose.pose.position.y = 0;
375 odom->pose.pose.position.z = 0;
378 nav_msgs::OdometryConstPtr odom_ptr(odom);
381 sensor_msgs::Imu *imu =
new sensor_msgs::Imu();
382 imu->orientation = request.geo_pose.orientation;
384 sensor_msgs::ImuConstPtr imu_ptr(imu);
415 mat.
getRPY(roll, pitch, yaw);
417 utm_orientation.
setRPY(roll, pitch, yaw);
426 robot_utm_pose = offset.
inverse() * gps_utm_pose;
433 " transform. Will assume navsat device is mounted at robot's origin");
436 robot_utm_pose = gps_utm_pose;
471 robot_odom_pose = gps_offset_rotated.
inverse() * gps_odom_pose;
476 " transform. Will not remove offset of navsat device from robot's origin.");
482 " transform. Will not remove offset of navsat device from robot's origin.");
492 ROS_WARN_STREAM_ONCE(
"NavSatFix message has empty frame_id. Will assume navsat device is mounted at robot's " 497 bool good_gps = (msg->status.status != sensor_msgs::NavSatStatus::STATUS_NO_FIX &&
498 !std::isnan(msg->altitude) &&
499 !std::isnan(msg->latitude) &&
500 !std::isnan(msg->longitude));
513 std::string utm_zone_tmp;
548 msg->header.frame_id,
555 double roll_offset = 0;
556 double pitch_offset = 0;
557 double yaw_offset = 0;
575 mat.
setRPY(0.0, 0.0, yaw_offset);
576 rpy_angles = mat * rpy_angles;
580 rpy_angles.getX() <<
", " << rpy_angles.getY() <<
", " << rpy_angles.getZ() <<
")");
592 " does not match world frame loaded from parameter (" <<
world_frame_id_ <<
")." 593 " World frame updating to " << msg->header.frame_id);
599 " Base link frame updating to " << msg->child_frame_id);
611 for (
size_t row = 0; row <
POSE_SIZE; ++row)
613 for (
size_t col = 0; col <
POSE_SIZE; ++col)
626 bool new_data =
false;
638 rot_6d.setIdentity();
642 rot_6d(rInd, 0) = rot.getRow(rInd).getX();
643 rot_6d(rInd, 1) = rot.getRow(rInd).getY();
644 rot_6d(rInd, 2) = rot.getRow(rInd).getZ();
645 rot_6d(rInd+POSITION_SIZE, 3) = rot.getRow(rInd).getX();
646 rot_6d(rInd+POSITION_SIZE, 4) = rot.getRow(rInd).getY();
647 rot_6d(rInd+POSITION_SIZE, 5) = rot.getRow(rInd).getZ();
657 filtered_gps.latitude,
658 filtered_gps.longitude);
659 filtered_gps.altitude = odom_as_utm.
getOrigin().getZ();
670 filtered_gps.position_covariance_type = sensor_msgs::NavSatFix::COVARIANCE_TYPE_KNOWN;
671 filtered_gps.status.status = sensor_msgs::NavSatStatus::STATUS_GBAS_FIX;
672 filtered_gps.header.frame_id =
"gps";
685 bool new_data =
false;
705 rot_6d.setIdentity();
709 rot_6d(rInd, 0) = rot.getRow(rInd).getX();
710 rot_6d(rInd, 1) = rot.getRow(rInd).getY();
711 rot_6d(rInd, 2) = rot.getRow(rInd).getZ();
712 rot_6d(rInd+POSITION_SIZE, 3) = rot.getRow(rInd).getX();
713 rot_6d(rInd+POSITION_SIZE, 4) = rot.getRow(rInd).getY();
714 rot_6d(rInd+POSITION_SIZE, 5) = rot.getRow(rInd).getZ();
721 tf2::toMsg(transformed_utm_robot, gps_odom.pose.pose);
722 gps_odom.pose.pose.position.z = (
zero_altitude_ ? 0.0 : gps_odom.pose.pose.position.z);
748 ROS_INFO_STREAM(
"Datum (latitude, longitude, altitude) is (" << std::fixed << msg->latitude <<
", " <<
749 msg->longitude <<
", " << msg->altitude <<
")");
750 ROS_INFO_STREAM(
"Datum UTM coordinate is (" << std::fixed << utm_x <<
", " << utm_y <<
")");
770 sensor_msgs::Imu *imu =
new sensor_msgs::Imu();
771 imu->orientation = msg->pose.pose.orientation;
772 imu->header.frame_id = msg->child_frame_id;
773 imu->header.stamp = msg->header.stamp;
774 sensor_msgs::ImuConstPtr imuPtr(imu);
const std::string & getMessage() const
void setRPY(const tf2Scalar &roll, const tf2Scalar &pitch, const tf2Scalar &yaw)
void getRPY(tf2Scalar &roll, tf2Scalar &pitch, tf2Scalar &yaw, unsigned int solution_number=1) const
void appendPrefix(std::string tfPrefix, std::string &frameId)
Utility method for appending tf2 prefixes cleanly.
static const Quaternion & getIdentity()
void publish(const boost::shared_ptr< M > &message) const
Subscriber subscribe(const std::string &topic, uint32_t queue_size, void(T::*fp)(M), T *obj, const TransportHints &transport_hints=TransportHints())
ServiceServer advertiseService(const std::string &service, bool(T::*srv_func)(MReq &, MRes &), T *obj)
Type const & getType() const
void setRPY(tf2Scalar roll, tf2Scalar pitch, tf2Scalar yaw)
Universal Transverse Mercator transforms. Functions to convert (spherical) latitude and longitude to ...
Duration & fromSec(double t)
const double RADIANS_PER_DEGREE
bool searchParam(const std::string &key, std::string &result) const
bool param(const std::string ¶m_name, T ¶m_val, const T &default_val) const
void fromMsg(const A &, B &b)
static void LLtoUTM(const double Lat, const double Long, double &UTMNorthing, double &UTMEasting, std::string &UTMZone, double &gamma)
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
#define ROS_WARN_STREAM(args)
#define ROS_DEBUG_STREAM(args)
double clampRotation(double rotation)
Utility method keeping RPY angles in the range [-pi, pi].
#define ROS_WARN_STREAM_THROTTLE(rate, args)
TF2SIMD_FORCE_INLINE Vector3 quatRotate(const Quaternion &rotation, const Vector3 &v)
#define ROS_INFO_STREAM(args)
const int POSE_SIZE
Pose and twist messages each contain six variables.
#define ROS_WARN_STREAM_ONCE(args)
bool getParam(const std::string &key, std::string &s) const
bool lookupTransformSafe(const tf2_ros::Buffer &buffer, const std::string &targetFrame, const std::string &sourceFrame, const ros::Time &time, const ros::Duration &timeout, tf2::Transform &targetFrameTrans, const bool silent=false)
Method for safely obtaining transforms.
bool hasParam(const std::string &key) const
#define ROS_ERROR_STREAM(args)
static void UTMtoLL(const double UTMNorthing, const double UTMEasting, const std::string &UTMZone, double &Lat, double &Long, double &gamma)
ROSCPP_DECL void spinOnce()
static bool waitForValid()
void quatToRPY(const tf2::Quaternion &quat, double &roll, double &pitch, double &yaw)
Utility method for converting quaternion to RPY.