diff --git a/CMakeLists.txt b/CMakeLists.txt index 5999aff96..88f5464f5 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -9,12 +9,17 @@ if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") add_compile_options(-Wall -Wextra -Wpedantic) endif() +if(NOT CMAKE_CONFIGURATION_TYPES AND NOT CMAKE_BUILD_TYPE) + message("${PROJECT_NAME}: You did not request a specific build type: selecting 'RelWithDebInfo'.") + set(CMAKE_BUILD_TYPE RelWithDebInfo) +endif() + find_package(ament_cmake REQUIRED) find_package(rcl REQUIRED) find_package(rclcpp REQUIRED) -find_package(geographic_msgs REQUIRED) find_package(diagnostic_msgs REQUIRED) find_package(diagnostic_updater REQUIRED) +find_package(geographic_msgs REQUIRED) find_package(geometry_msgs REQUIRED) find_package(nav_msgs REQUIRED) find_package(rosidl_default_generators REQUIRED) @@ -32,10 +37,12 @@ pkg_check_modules(YAML_CPP yaml-cpp) set(library_name rl_lib) rosidl_generate_interfaces(${PROJECT_NAME} + "srv/FromLL.srv" "srv/GetState.srv" "srv/SetDatum.srv" "srv/SetPose.srv" "srv/ToggleFilterProcessing.srv" + "srv/ToLL.srv" DEPENDENCIES builtin_interfaces geometry_msgs diff --git a/doc/navsat_transform_node.rst b/doc/navsat_transform_node.rst index 58b613d01..6218f9b66 100644 --- a/doc/navsat_transform_node.rst +++ b/doc/navsat_transform_node.rst @@ -47,6 +47,15 @@ If *true*, ``navsat_transform_node`` will wait to get a datum from either: * The ``datum`` parameter * The ``set_datum`` service +~broadcast_utm_transform_as_parent_frame +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +If *true*, ``navsat_transform_node`` will publish the utm->world_frame transform instead of the world_frame->utm transform. +Note that for the transform to be published ``broadcast_utm_transform`` also has to be set to *true*. + +~transform_timeout +^^^^^^^^^^^^^^^^^^ +This parameter specifies how long we would like to wait if a transformation is not available yet. Defaults to 0 if not set. The value 0 means we just get us the latest available (see ``tf2`` implementation) transform. + Subscribed Topics ================= * ``imu/data`` A `sensor_msgs/Imu `_ message with orientation data @@ -63,4 +72,4 @@ Published Topics Published Transforms ==================== -* ``world_frame->utm`` (optional) - If the ``broadcast_utm_transform`` parameter is set to *true*, ``navsat_transform_node`` calculates a transform from the *utm* frame to the ``frame_id`` of the input odometry data. By default, the *utm* frame is published as a child of the odometry frame by using the inverse transform. With use of the ``broadcast_utm_as_parent_frame`` parameter, the *utm* frame will be published as a parent of the odometry frame. This is useful if you have multiple robots within one TF tree. +* ``world_frame->utm`` (optional) - If the ``broadcast_utm_transform`` parameter is set to *true*, ``navsat_transform_node`` calculates a transform from the *utm* frame to the ``frame_id`` of the input odometry data. By default, the *utm* frame is published as a child of the odometry frame by using the inverse transform. With use of the ``broadcast_utm_transform_as_parent_frame`` parameter, the *utm* frame will be published as a parent of the odometry frame. This is useful if you have multiple robots within one TF tree. diff --git a/doc/state_estimation_nodes.rst b/doc/state_estimation_nodes.rst index dc8d705f7..e49892842 100644 --- a/doc/state_estimation_nodes.rst +++ b/doc/state_estimation_nodes.rst @@ -43,6 +43,7 @@ Specific parameters: * ``~map_frame`` * ``~odom_frame`` * ``~base_link_frame`` +* ``~base_link_output_frame`` * ``~world_frame`` These parameters define the operating "mode" for ``robot_localization``. `REP-105 `_ specifies three principal coordinate frames: *map*, *odom*, and *base_link*. *base_link* is the coordinate frame that is affixed to the robot. The robot's position in the *odom* frame will drift over time, but is accurate in the short term and should be continuous. The *map* frame, like the *odom* frame, is a world-fixed coordinate frame, and while it contains the most globally accurate position estimate for your robot, it is subject to discrete jumps, e.g., due to the fusion of GPS data. Here is how to use these parameters: @@ -50,6 +51,7 @@ These parameters define the operating "mode" for ``robot_localization``. `REP-10 1. Set the ``map_frame``, ``odom_frame``, and ``base_link_frame`` parameters to the appropriate frame names for your system. .. note:: If your system does not have a ``map_frame``, just remove it, and make sure ``world_frame`` is set to the value of ``odom_frame``. + .. note:: If you are running multiple EKF instances and would like to "override" the output transform and message to have this frame for its ``child_frame_id``, you may set this. The ``base_link_output_frame`` is optional and will default to the ``base_link_frame``. This helps to enable disconnected TF trees when multiple EKF instances are being run. When the final state is computed, we "override" the output transform and message to have this frame for its ``child_frame_id``. 2. If you are only fusing continuous position data such as wheel encoder odometry, visual odometry, or IMU data, set ``world_frame`` to your ``odom_frame`` value. This is the default behavior for the state estimation nodes in ``robot_localization``, and the most common use for it. 3. If you are fusing global absolute position data that is subject to discrete jumps (e.g., GPS or position updates from landmark observations) then: @@ -57,7 +59,7 @@ These parameters define the operating "mode" for ``robot_localization``. `REP-10 i. Set your ``world_frame`` to your ``map_frame`` value ii. **Make sure** something else is generating the *odom->base_link* transform. This can even be another instance of a ``robot_localization`` state estimation node. However, that instance should *not* fuse the global data. -The default values for ``map_frame``, ``odom_frame``, and ``base_link_frame`` are *map*, *odom,* and *base_link,* respectively. The ``world_frame`` parameter defaults to the value of ``odom_frame``. +The default values for ``map_frame``, ``odom_frame``, and ``base_link_frame`` are *map*, *odom,* and *base_link,* respectively. The ``base_link_output_frame`` parameter defaults to the value of ``base_link_frame``. The ``world_frame`` parameter defaults to the value of ``odom_frame``. ~transform_time_offset ^^^^^^^^^^^^^^^^^^^^^^ @@ -286,6 +288,14 @@ The estimate covariance, commonly denoted *P*, defines the error in the current ^^^^^^^^^^^^^^^^^^^ If set to *true* and ``ros::Time::isSimTime()`` is *true*, the filter will reset to its uninitialized state when a jump back in time is detected on a topic. This is useful when working with bag data, in that the bag can be restarted without restarting the node. +~predict_to_current_time +^^^^^^^^^^^^^^^^^^^^^^^^ +If set to *true*, the filter predicts and corrects up to the time of the latest measurement (by default) but will now also predict up to the current time step. + +~disabled_at_startup +^^^^^^^^^^^^^^^^^^^^ +If set to *true* will not run the filter on start. + Node-specific Parameters ------------------------ The standard and advanced parameters are common to all state estimation nodes in ``robot_localization``. This section details parameters that are unique to their respective state estimation nodes. @@ -319,4 +329,4 @@ Published Transforms Services ======== -* ``set_pose`` - By issuing a `geometry_msgs/PoseWithCovarianceStamped `_ message to the ``set_pose`` topic, users can manually set the state of the filter. This is useful for resetting the filter during testing, and allows for interaction with ``rviz``. Alternatively, the state estimation nodes advertise a ``SetPose`` service, whose type is `robot_localization/SetPose `_.rejection +* ``set_pose`` - By issuing a `geometry_msgs/PoseWithCovarianceStamped `_ message to the ``set_pose`` topic, users can manually set the state of the filter. This is useful for resetting the filter during testing, and allows for interaction with ``rviz``. Alternatively, the state estimation nodes advertise a ``SetPose`` service, whose type is `robot_localization/SetPose `_. diff --git a/include/robot_localization/filter_common.hpp b/include/robot_localization/filter_common.hpp index a9e172594..d849c4184 100644 --- a/include/robot_localization/filter_common.hpp +++ b/include/robot_localization/filter_common.hpp @@ -90,6 +90,7 @@ const int POSE_SIZE = 6; const int TWIST_SIZE = 6; const int POSITION_SIZE = 3; const int ORIENTATION_SIZE = 3; +const int LINEAR_VELOCITY_SIZE = 3; const int ACCELERATION_SIZE = 3; /** diff --git a/include/robot_localization/navsat_conversions.hpp b/include/robot_localization/navsat_conversions.hpp index 7875e7a54..4b56b5615 100644 --- a/include/robot_localization/navsat_conversions.hpp +++ b/include/robot_localization/navsat_conversions.hpp @@ -204,13 +204,17 @@ static inline char UTMLetterDesignator(double Lat) * East Longitudes are positive, West longitudes are negative. * North latitudes are positive, South latitudes are negative * Lat and Long are in fractional degrees + * Meridian convergence is computed as for Spherical Transverse Mercator, + * which gives quite good approximation. + * + * @param[out] gamma meridian convergence at point (degrees). * * Written by Chuck Gantz- chuck.gantz@globalstar.com */ static inline void LLtoUTM( const double Lat, const double Long, double & UTMNorthing, double & UTMEasting, - std::string & UTMZone) + std::string & UTMZone, double & gamma) { double a = WGS84_A; double eccSquared = UTM_E2; @@ -253,7 +257,9 @@ static inline void LLtoUTM( // Compute the UTM Zone from the latitude and longitude char zone_buf[] = {0, 0, 0, 0}; - snprintf(zone_buf, sizeof(zone_buf), "%d%c", ZoneNumber, + // We &0x3fU to let GCC know the size of ZoneNumber. In this case, it's under + // 63 (6bits) + snprintf(zone_buf, sizeof(zone_buf), "%d%c", ZoneNumber & 0x3fU, UTMLetterDesignator(Lat)); UTMZone = std::string(zone_buf); @@ -290,25 +296,49 @@ static inline void LLtoUTM( (61 - 58 * T + T * T + 600 * C - 330 * eccPrimeSquared) * A * A * A * A * A * A / 720))); + gamma = atan(tan(LongRad - LongOriginRad) * sin(LatRad)) * DEGREES_PER_RADIAN; + if (Lat < 0) { // 10000000 meter offset for southern hemisphere UTMNorthing += 10000000.0; } } +/** + * Convert lat/long to UTM coords. Equations from USGS Bulletin 1532 + * + * East Longitudes are positive, West longitudes are negative. + * North latitudes are positive, South latitudes are negative + * Lat and Long are in fractional degrees + * + * Written by Chuck Gantz- chuck.gantz@globalstar.com + */ +static inline void LLtoUTM( + const double Lat, const double Long, + double & UTMNorthing, double & UTMEasting, + std::string & UTMZone) +{ + double gamma; + LLtoUTM(Lat, Long, UTMNorthing, UTMEasting, UTMZone, gamma); +} + /** * Converts UTM coords to lat/long. Equations from USGS Bulletin 1532 * * East Longitudes are positive, West longitudes are negative. * North latitudes are positive, South latitudes are negative * Lat and Long are in fractional degrees. + * Meridian convergence is computed as for Spherical Transverse Mercator, + * which gives quite good approximation. + * + * @param[out] gamma meridian convergence at point (degrees). * * Written by Chuck Gantz- chuck.gantz@globalstar.com */ static inline void UTMtoLL( const double UTMNorthing, const double UTMEasting, const std::string & UTMZone, double & Lat, - double & Long) + double & Long, double & gamma) { double k0 = UTM_K0; double a = WGS84_A; @@ -367,6 +397,25 @@ static inline void UTMtoLL( D * D * D * D * D / 120) / cos(phi1Rad)); Long = LongOrigin + Long * DEGREES_PER_RADIAN; + + gamma = atan(tanh(x / (k0 * a)) * tan(y / (k0 * a))) * DEGREES_PER_RADIAN; +} + +/** +* Converts UTM coords to lat/long. Equations from USGS Bulletin 1532 +* +* East Longitudes are positive, West longitudes are negative. +* North latitudes are positive, South latitudes are negative +* Lat and Long are in fractional degrees. +* +* Written by Chuck Gantz- chuck.gantz@globalstar.com +*/ +static inline void UTMtoLL( + const double UTMNorthing, const double UTMEasting, + const std::string & UTMZone, double & Lat, double & Long) +{ + double gamma; + UTMtoLL(UTMNorthing, UTMEasting, UTMZone, Lat, Long, gamma); } } // namespace navsat_conversions diff --git a/include/robot_localization/navsat_transform.hpp b/include/robot_localization/navsat_transform.hpp index 8bfeaae09..984c7d945 100644 --- a/include/robot_localization/navsat_transform.hpp +++ b/include/robot_localization/navsat_transform.hpp @@ -34,6 +34,8 @@ #define ROBOT_LOCALIZATION__NAVSAT_TRANSFORM_HPP_ #include +#include +#include #include #include @@ -83,6 +85,18 @@ class NavSatTransform : public rclcpp::Node request, std::shared_ptr); + //! @brief Callback for the to Lat Long service + //! + bool toLLCallback( + const std::shared_ptr request, + std::shared_ptr response); + + //! @brief Callback for the from Lat Long service + //! + bool fromLLCallback( + const std::shared_ptr request, + std::shared_ptr response); + /** * @brief Given the pose of the navsat sensor in the UTM frame, removes the * offset from the vehicle's centroid and returns the UTM-frame pose of said @@ -147,6 +161,20 @@ class NavSatTransform : public rclcpp::Node */ void setTransformOdometry(const nav_msgs::msg::Odometry::SharedPtr & msg); + /** + * @brief Transforms the passed in pose from utm to map frame + * @param[in] utm_pose the pose in utm frame to use to transform + */ + nav_msgs::msg::Odometry utmToMap(const tf2::Transform & utm_pose) const; + + /** + * @brief Transforms the passed in point from map frame to lat/long + * @param[in] point the point in map frame to use to transform + */ + void mapToLL( + const tf2::Vector3 & point, double & latitude, double & longitude, + double & altitude) const; + /** * @brief Frame ID of the robot's body frame * @@ -171,6 +199,16 @@ class NavSatTransform : public rclcpp::Node */ rclcpp::Service::SharedPtr datum_srv_; + /** + * @brief Service for to Lat Long + */ + rclcpp::Service::SharedPtr to_ll_srv_; + + /** + * @brief Service for from Lat Long + */ + rclcpp::Service::SharedPtr from_ll_srv_; + /** * @brief Navsatfix publisher */ @@ -337,9 +375,14 @@ class NavSatTransform : public rclcpp::Node tf2_ros::StaticTransformBroadcaster utm_broadcaster_; /** - * @brief Stores the yaw we need to compute the transform + * @brief UTM's meridian convergence + * + * Angle between projected meridian (True North) and UTM's grid Y-axis. + * For UTM projection (Ellipsoidal Transverse Mercator) it is zero on the + * equator and non-zero everywhere else. It increases as the poles are + * approached or as we're getting farther from central meridian. */ - double utm_odom_tf_yaw_; + double utm_meridian_convergence_; /** * @brief Holds the UTM->odom transform diff --git a/include/robot_localization/ros_filter.hpp b/include/robot_localization/ros_filter.hpp index 1a5849e2b..bcd82461f 100644 --- a/include/robot_localization/ros_filter.hpp +++ b/include/robot_localization/ros_filter.hpp @@ -554,6 +554,14 @@ class RosFilter : public rclcpp::Node //! std::string base_link_frame_id_; + //! @brief tf frame name for the robot's body frame + //! + //! When the final state is computed, we "override" the output transform and + //! message to have this frame for its child_frame_id. This helps to enable + //! disconnected TF trees when multiple EKF instances are being run. + //! + std::string base_link_output_frame_id_; + //! @brief tf frame name for the robot's map (world-fixed) frame //! std::string map_frame_id_; diff --git a/src/ekf.cpp b/src/ekf.cpp index cd3f7df95..85945dd1a 100644 --- a/src/ekf.cpp +++ b/src/ekf.cpp @@ -228,7 +228,6 @@ void Ekf::predict( double x_vel = state_(StateMemberVx); double y_vel = state_(StateMemberVy); double z_vel = state_(StateMemberVz); - double roll_vel = state_(StateMemberVroll); double pitch_vel = state_(StateMemberVpitch); double yaw_vel = state_(StateMemberVyaw); double x_acc = state_(StateMemberAx); @@ -238,6 +237,8 @@ void Ekf::predict( // We'll need these trig calculations a lot. double sp = ::sin(pitch); double cp = ::cos(pitch); + double cpi = 1.0 / cp; + double tp = sp * cpi; double sr = ::sin(roll); double cr = ::cos(roll); @@ -279,24 +280,13 @@ void Ekf::predict( 0.5 * transfer_function_(StateMemberZ, StateMemberVy) * delta_sec; transfer_function_(StateMemberZ, StateMemberAz) = 0.5 * transfer_function_(StateMemberZ, StateMemberVz) * delta_sec; - transfer_function_(StateMemberRoll, StateMemberVroll) = - transfer_function_(StateMemberX, StateMemberVx); - transfer_function_(StateMemberRoll, StateMemberVpitch) = - transfer_function_(StateMemberX, StateMemberVy); - transfer_function_(StateMemberRoll, StateMemberVyaw) = - transfer_function_(StateMemberX, StateMemberVz); - transfer_function_(StateMemberPitch, StateMemberVroll) = - transfer_function_(StateMemberY, StateMemberVx); - transfer_function_(StateMemberPitch, StateMemberVpitch) = - transfer_function_(StateMemberY, StateMemberVy); - transfer_function_(StateMemberPitch, StateMemberVyaw) = - transfer_function_(StateMemberY, StateMemberVz); - transfer_function_(StateMemberYaw, StateMemberVroll) = - transfer_function_(StateMemberZ, StateMemberVx); - transfer_function_(StateMemberYaw, StateMemberVpitch) = - transfer_function_(StateMemberZ, StateMemberVy); - transfer_function_(StateMemberYaw, StateMemberVyaw) = - transfer_function_(StateMemberZ, StateMemberVz); + transfer_function_(StateMemberRoll, StateMemberVroll) = delta_sec; + transfer_function_(StateMemberRoll, StateMemberVpitch) = sr * tp * delta_sec; + transfer_function_(StateMemberRoll, StateMemberVyaw) = cr * tp * delta_sec; + transfer_function_(StateMemberPitch, StateMemberVpitch) = cr * delta_sec; + transfer_function_(StateMemberPitch, StateMemberVyaw) = -sr * delta_sec; + transfer_function_(StateMemberYaw, StateMemberVpitch) = sr * cpi * delta_sec; + transfer_function_(StateMemberYaw, StateMemberVyaw) = cr * cpi * delta_sec; transfer_function_(StateMemberVx, StateMemberAx) = delta_sec; transfer_function_(StateMemberVy, StateMemberAy) = delta_sec; transfer_function_(StateMemberVz, StateMemberAz) = delta_sec; @@ -312,7 +302,7 @@ void Ekf::predict( z_coeff = -cy * sp * sr + sy * cr; double dFx_dR = (y_coeff * y_vel + z_coeff * z_vel) * delta_sec + (y_coeff * y_acc + z_coeff * z_acc) * one_half_at_squared; - double dFR_dR = 1 + (y_coeff * pitch_vel + z_coeff * yaw_vel) * delta_sec; + double dFR_dR = 1.0 + (cr * tp * pitch_vel - sr * tp * yaw_vel) * delta_sec; x_coeff = -cy * sp; y_coeff = cy * cp * sr; @@ -322,7 +312,7 @@ void Ekf::predict( (x_coeff * x_acc + y_coeff * y_acc + z_coeff * z_acc) * one_half_at_squared; double dFR_dP = - (x_coeff * roll_vel + y_coeff * pitch_vel + z_coeff * yaw_vel) * delta_sec; + (cpi * cpi * sr * pitch_vel + cpi * cpi * cr * yaw_vel) * delta_sec; x_coeff = -sy * cp; y_coeff = -sy * sp * sr - cy * cr; @@ -331,14 +321,12 @@ void Ekf::predict( (x_coeff * x_vel + y_coeff * y_vel + z_coeff * z_vel) * delta_sec + (x_coeff * x_acc + y_coeff * y_acc + z_coeff * z_acc) * one_half_at_squared; - double dFR_dY = - (x_coeff * roll_vel + y_coeff * pitch_vel + z_coeff * yaw_vel) * delta_sec; y_coeff = sy * sp * cr - cy * sr; z_coeff = -sy * sp * sr - cy * cr; double dFy_dR = (y_coeff * y_vel + z_coeff * z_vel) * delta_sec + (y_coeff * y_acc + z_coeff * z_acc) * one_half_at_squared; - double dFP_dR = (y_coeff * pitch_vel + z_coeff * yaw_vel) * delta_sec; + double dFP_dR = (-sr * pitch_vel - cr * yaw_vel) * delta_sec; x_coeff = -sy * sp; y_coeff = sy * cp * sr; @@ -347,9 +335,6 @@ void Ekf::predict( (x_coeff * x_vel + y_coeff * y_vel + z_coeff * z_vel) * delta_sec + (x_coeff * x_acc + y_coeff * y_acc + z_coeff * z_acc) * one_half_at_squared; - double dFP_dP = - 1 + - (x_coeff * roll_vel + y_coeff * pitch_vel + z_coeff * yaw_vel) * delta_sec; x_coeff = cy * cp; y_coeff = cy * sp * sr - sy * cr; @@ -358,14 +343,12 @@ void Ekf::predict( (x_coeff * x_vel + y_coeff * y_vel + z_coeff * z_vel) * delta_sec + (x_coeff * x_acc + y_coeff * y_acc + z_coeff * z_acc) * one_half_at_squared; - double dFP_dY = - (x_coeff * roll_vel + y_coeff * pitch_vel + z_coeff * yaw_vel) * delta_sec; y_coeff = cp * cr; z_coeff = -cp * sr; double dFz_dR = (y_coeff * y_vel + z_coeff * z_vel) * delta_sec + (y_coeff * y_acc + z_coeff * z_acc) * one_half_at_squared; - double dFY_dR = (y_coeff * pitch_vel + z_coeff * yaw_vel) * delta_sec; + double dFY_dR = (cr * cpi * pitch_vel - sr * cpi * yaw_vel) * delta_sec; x_coeff = -cp; y_coeff = -sp * sr; @@ -375,7 +358,7 @@ void Ekf::predict( (x_coeff * x_acc + y_coeff * y_acc + z_coeff * z_acc) * one_half_at_squared; double dFY_dP = - (x_coeff * roll_vel + y_coeff * pitch_vel + z_coeff * yaw_vel) * delta_sec; + (sr * tp * cpi * pitch_vel - cr * tp * cpi * yaw_vel) * delta_sec; // Much of the transfer function Jacobian is identical to the transfer // function @@ -390,10 +373,7 @@ void Ekf::predict( transfer_function_jacobian_(StateMemberZ, StateMemberPitch) = dFz_dP; transfer_function_jacobian_(StateMemberRoll, StateMemberRoll) = dFR_dR; transfer_function_jacobian_(StateMemberRoll, StateMemberPitch) = dFR_dP; - transfer_function_jacobian_(StateMemberRoll, StateMemberYaw) = dFR_dY; transfer_function_jacobian_(StateMemberPitch, StateMemberRoll) = dFP_dR; - transfer_function_jacobian_(StateMemberPitch, StateMemberPitch) = dFP_dP; - transfer_function_jacobian_(StateMemberPitch, StateMemberYaw) = dFP_dY; transfer_function_jacobian_(StateMemberYaw, StateMemberRoll) = dFY_dR; transfer_function_jacobian_(StateMemberYaw, StateMemberPitch) = dFY_dP; diff --git a/src/navsat_transform.cpp b/src/navsat_transform.cpp index 8168676e4..20ecf13ee 100644 --- a/src/navsat_transform.cpp +++ b/src/navsat_transform.cpp @@ -73,7 +73,7 @@ NavSatTransform::NavSatTransform(const rclcpp::NodeOptions & options) use_manual_datum_(false), use_odometry_yaw_(false), utm_broadcaster_(*this), - utm_odom_tf_yaw_(0.0), + utm_meridian_convergence_(0.0), utm_zone_(""), world_frame_id_("odom"), yaw_offset_(0.0), @@ -108,6 +108,11 @@ NavSatTransform::NavSatTransform(const rclcpp::NodeOptions & options) datum_srv_ = this->create_service( "datum", std::bind(&NavSatTransform::datumCallback, this, _1, _2)); + to_ll_srv_ = this->create_service( + "toLL", std::bind(&NavSatTransform::toLLCallback, this, _1, _2)); + from_ll_srv_ = this->create_service( + "fromLL", std::bind(&NavSatTransform::fromLLCallback, this, _1, _2)); + std::vector datum_vals; if (use_manual_datum_ && this->get_parameter("datum", datum_vals)) { double datum_lat = 0.0; @@ -218,27 +223,31 @@ void NavSatTransform::computeTransform() * However, all the nodes in robot_localization assume that orientation * data, including that reported by IMUs, is reported in an ENU frame, with * a 0 yaw value being reported when facing east and increasing - * counter-clockwise (i.e., towards north). Conveniently, this aligns with - * the UTM grid, where X is east and Y is north. However, we have two - * additional considerations: + * counter-clockwise (i.e., towards north). To make the world frame ENU + * aligned, where X is east and Y is north, we have to take into account + * three additional considerations: * 1. The IMU may have its non-ENU frame data transformed to ENU, but - * there's a possibility that its data has not been corrected for magnetic - * declination. We need to account for this. A positive magnetic - * declination is counter-clockwise in an ENU frame. Therefore, if - * we have a magnetic declination of N radians, then when the sensor - * is facing a heading of N, it reports 0. Therefore, we need to add - * the declination angle. + * there's a possibility that its data has not been corrected for + * magnetic declination. We need to account for this. A positive + * magnetic declination is counter-clockwise in an ENU frame. + * Therefore, if we have a magnetic declination of N radians, then when + * the sensor is facing a heading of N, it reports 0. Therefore, we + * need to add the declination angle. * 2. To account for any other offsets that may not be accounted for by - * the IMU driver or any interim processing node, we expose a yaw offset - * that lets users work with navsat_transform_node. + * the IMU driver or any interim processing node, we expose a yaw + * offset that lets users work with navsat_transform_node. + * 3. UTM grid isn't aligned with True East\North. To account for the + * difference we need to add meridian convergence angle. */ - imu_yaw += (magnetic_declination_ + yaw_offset_); + imu_yaw += (magnetic_declination_ + yaw_offset_ + + utm_meridian_convergence_); RCLCPP_INFO( this->get_logger(), - "Corrected for magnetic declination of %d" - "and user-specified offset of %d Transform heading factor is now %d", - magnetic_declination_, yaw_offset_, imu_yaw); + "Corrected for magnetic declination of %g, " + "user-specified offset of %g and meridian convergence of %g. " + "Transform heading factor is now %g", + magnetic_declination_, yaw_offset_, utm_meridian_convergence_, imu_yaw); // Convert to tf-friendly structures tf2::Quaternion imu_quat; @@ -329,6 +338,98 @@ bool NavSatTransform::datumCallback( return true; } +bool NavSatTransform::toLLCallback( + const std::shared_ptr request, + std::shared_ptr response) +{ + if (!transform_good_) { + return false; + } + // tf2::Vector3 point; + // tf2::fromMsg(request.map_point, point); + tf2::Vector3 point(request->map_point.x, request->map_point.y, + request->map_point.z); + mapToLL(point, response->ll_point.latitude, response->ll_point.longitude, + response->ll_point.altitude); + + return true; +} + +bool NavSatTransform::fromLLCallback( + const std::shared_ptr request, + std::shared_ptr response) +{ + double altitude = request->ll_point.altitude; + double longitude = request->ll_point.longitude; + double latitude = request->ll_point.latitude; + + tf2::Transform utm_pose; + + double utmY, utmX; + + std::string utm_zone_tmp; + navsat_conversions::LLtoUTM(latitude, longitude, utmY, utmX, utm_zone_tmp); + + utm_pose.setOrigin(tf2::Vector3(utmX, utmY, altitude)); + + nav_msgs::msg::Odometry gps_odom; + + if (!transform_good_) { + return false; + } + + response->map_point = utmToMap(utm_pose).pose.pose.position; + + return true; +} + +nav_msgs::msg::Odometry NavSatTransform::utmToMap( + const tf2::Transform & utm_pose) const +{ + nav_msgs::msg::Odometry gps_odom{}; + + tf2::Transform transformed_utm_gps{}; + + transformed_utm_gps.mult(utm_world_transform_, utm_pose); + transformed_utm_gps.setRotation(tf2::Quaternion::getIdentity()); + + // Set header information stamp because we would like to know the robot's + // position at that timestamp + gps_odom.header.frame_id = world_frame_id_; + gps_odom.header.stamp = gps_update_time_; + + // Now fill out the message. Set the orientation to the identity. + tf2::toMsg(transformed_utm_gps, gps_odom.pose.pose); + gps_odom.pose.pose.position.z = (zero_altitude_ ? 0.0 : + gps_odom.pose.pose.position.z); + + return gps_odom; +} + +void NavSatTransform::mapToLL( + const tf2::Vector3 & point, + double & latitude, + double & longitude, + double & altitude) const +{ + tf2::Transform odom_as_utm{}; + + tf2::Transform pose{}; + pose.setOrigin(point); + pose.setRotation(tf2::Quaternion::getIdentity()); + + odom_as_utm.mult(utm_world_trans_inverse_, pose); + odom_as_utm.setRotation(tf2::Quaternion::getIdentity()); + + // Now convert the data back to lat/long and place into the message + navsat_conversions::UTMtoLL(odom_as_utm.getOrigin().getY(), + odom_as_utm.getOrigin().getX(), + utm_zone_, + latitude, + longitude); + altitude = odom_as_utm.getOrigin().getZ(); +} + void NavSatTransform::getRobotOriginUtmPose( const tf2::Transform & gps_utm_pose, tf2::Transform & robot_utm_pose, const rclcpp::Time & transform_time) @@ -351,7 +452,7 @@ void NavSatTransform::getRobotOriginUtmPose( double pitch; double yaw; mat.getRPY(roll, pitch, yaw); - yaw += (magnetic_declination_ + yaw_offset_); + yaw += (magnetic_declination_ + yaw_offset_ + utm_meridian_convergence_); utm_orientation.setRPY(roll, pitch, yaw); // Rotate the GPS linear offset by the orientation @@ -541,10 +642,8 @@ bool NavSatTransform::prepareFilteredGps( bool new_data = false; if (transform_good_ && odom_updated_) { - tf2::Transform odom_as_utm; - - odom_as_utm.mult(utm_world_trans_inverse_, latest_world_pose_); - odom_as_utm.setRotation(tf2::Quaternion::getIdentity()); + mapToLL(latest_world_pose_.getOrigin(), filtered_gps.latitude, + filtered_gps.longitude, filtered_gps.altitude); // Rotate the covariance as well tf2::Matrix3x3 rot(utm_world_trans_inverse_.getRotation()); @@ -564,12 +663,6 @@ bool NavSatTransform::prepareFilteredGps( latest_odom_covariance_ = rot_6d * latest_odom_covariance_.eval() * rot_6d.transpose(); - // Now convert the data back to lat/long and place into the message - navsat_conversions::UTMtoLL(odom_as_utm.getOrigin().getY(), - odom_as_utm.getOrigin().getX(), utm_zone_, - filtered_gps.latitude, filtered_gps.longitude); - filtered_gps.altitude = odom_as_utm.getOrigin().getZ(); - // Copy the measurement's covariance matrix back for (size_t i = 0; i < POSITION_SIZE; i++) { for (size_t j = 0; j < POSITION_SIZE; j++) { @@ -582,7 +675,7 @@ bool NavSatTransform::prepareFilteredGps( sensor_msgs::msg::NavSatFix::COVARIANCE_TYPE_KNOWN; filtered_gps.status.status = sensor_msgs::msg::NavSatStatus::STATUS_GBAS_FIX; - filtered_gps.header.frame_id = "gps"; + filtered_gps.header.frame_id = base_link_frame_id_; filtered_gps.header.stamp = odom_update_time_; // Mark this GPS as used @@ -598,15 +691,10 @@ bool NavSatTransform::prepareGpsOdometry(nav_msgs::msg::Odometry & gps_odom) bool new_data = false; if (transform_good_ && gps_updated_ && odom_updated_) { - tf2::Transform transformed_utm_gps; - - transformed_utm_gps.mult(utm_world_transform_, latest_utm_pose_); - transformed_utm_gps.setRotation(tf2::Quaternion::getIdentity()); + gps_odom = utmToMap(latest_utm_pose_); - // Set header information stamp because we would like to know the robot's - // position at that timestamp - gps_odom.header.frame_id = world_frame_id_; - gps_odom.header.stamp = gps_update_time_; + tf2::Transform transformed_utm_gps; + tf2::fromMsg(gps_odom.pose.pose, transformed_utm_gps); // Want the pose of the vehicle origin, not the GPS tf2::Transform transformed_utm_robot; @@ -660,7 +748,8 @@ void NavSatTransform::setTransformGps( double utm_x = 0; double utm_y = 0; navsat_conversions::LLtoUTM(msg->latitude, msg->longitude, utm_y, utm_x, - utm_zone_); + utm_zone_, utm_meridian_convergence_); + utm_meridian_convergence_ *= navsat_conversions::RADIANS_PER_DEGREE; RCLCPP_INFO( this->get_logger(), "Datum (latitude, longitude, altitude) is (%d, %d, %d)", @@ -678,6 +767,9 @@ void NavSatTransform::setTransformOdometry( tf2::fromMsg(msg->pose.pose, transform_world_pose_); has_transform_odom_ = true; + // TODO(anyone) add back in Eloquent + // ROS_INFO_STREAM_ONCE("Initial odometry pose is " << transform_world_pose_); + // Users can optionally use the (potentially fused) heading from // the odometry source, which may have multiple fused sources of // heading data, and so would act as a better heading for the diff --git a/src/ros_filter.cpp b/src/ros_filter.cpp index 3be08c8c7..9bf524680 100644 --- a/src/ros_filter.cpp +++ b/src/ros_filter.cpp @@ -399,7 +399,7 @@ bool RosFilter::getFilteredOdometryMessage(nav_msgs::msg::Odometry & message) message.header.stamp = filter_.getLastMeasurementTime(); message.header.frame_id = world_frame_id_; - message.child_frame_id = base_link_frame_id_; + message.child_frame_id = base_link_output_frame_id_; } return filter_.getInitializedStatus(); @@ -433,7 +433,7 @@ bool RosFilter::getFilteredAccelMessage( // Fill header information message.header.stamp = rclcpp::Time(filter_.getLastMeasurementTime()); - message.header.frame_id = base_link_frame_id_; + message.header.frame_id = base_link_output_frame_id_; } return filter_.getInitializedStatus(); @@ -492,7 +492,7 @@ void RosFilter::imuCallback( for (size_t i = 0; i < ORIENTATION_SIZE; i++) { for (size_t j = 0; j < ORIENTATION_SIZE; j++) { pos_ptr->pose.covariance[POSE_SIZE * (i + ORIENTATION_SIZE) + - (j + ORIENTATION_SIZE)] = + (j + ORIENTATION_SIZE)] = msg->orientation_covariance[ORIENTATION_SIZE * i + j]; } } @@ -524,7 +524,7 @@ void RosFilter::imuCallback( for (size_t i = 0; i < ORIENTATION_SIZE; i++) { for (size_t j = 0; j < ORIENTATION_SIZE; j++) { twist_ptr->twist.covariance[TWIST_SIZE * (i + ORIENTATION_SIZE) + - (j + ORIENTATION_SIZE)] = + (j + ORIENTATION_SIZE)] = msg->angular_velocity_covariance[ORIENTATION_SIZE * i + j]; } } @@ -740,6 +740,8 @@ void RosFilter::loadParams() odom_frame_id_ = this->declare_parameter("odom_frame", std::string("odom")); base_link_frame_id_ = this->declare_parameter("base_link_frame", std::string("base_link")); + base_link_output_frame_id_ = this->declare_parameter("base_link_frame_output", + base_link_frame_id_); /* * These parameters are designed to enforce compliance with REP-105: @@ -770,11 +772,14 @@ void RosFilter::loadParams() if (map_frame_id_ == odom_frame_id_ || odom_frame_id_ == base_link_frame_id_ || - map_frame_id_ == base_link_frame_id_) + map_frame_id_ == base_link_frame_id_ || + odom_frame_id_ == base_link_output_frame_id_ || + map_frame_id_ == base_link_output_frame_id_) { std::cerr << "Invalid frame configuration! The values for map_frame, odom_frame, " - "and base_link_frame must be unique." << + "and base_link_frame must be unique. If using a base_link_frame_output " + "values, it must not match the map_frame or odom_frame." "\n"; } @@ -787,6 +792,7 @@ void RosFilter::loadParams() filter_utilities::appendPrefix(tf_prefix, map_frame_id_); filter_utilities::appendPrefix(tf_prefix, odom_frame_id_); filter_utilities::appendPrefix(tf_prefix, base_link_frame_id_); + filter_utilities::appendPrefix(tf_prefix, base_link_output_frame_id_); filter_utilities::appendPrefix(tf_prefix, world_frame_id_); } @@ -954,7 +960,8 @@ void RosFilter::loadParams() RF_DEBUG("tf_prefix is " << tf_prefix << "\nmap_frame is " << map_frame_id_ << "\nodom_frame is " << odom_frame_id_ << "\nbase_link_frame is " << - base_link_frame_id_ << "\nworld_frame is " << world_frame_id_ << + base_link_frame_id_ << "\nbase_link_output_frame is " << + base_link_output_frame_id_ << "\nworld_frame is " << world_frame_id_ << "\ntransform_time_offset is " << filter_utilities::toSec(tf_time_offset_) << "\ntransform_timeout is " << filter_utilities::toSec(tf_timeout_) << @@ -1356,7 +1363,7 @@ void RosFilter::loadParams() if (more_params) { bool differential = this->declare_parameter(imu_topic_name + std::string("_differential"), - differential); + false); // Determine if we want to integrate this sensor relatively bool relative = this->declare_parameter(imu_topic_name + std::string("_relative"), false); @@ -1400,18 +1407,48 @@ void RosFilter::loadParams() // update configuration (as this contains pose information) std::vector update_vec = loadUpdateConfig(imu_topic_name); + // sanity checks for update config settings + std::vector position_update_vec(update_vec.begin() + POSITION_OFFSET, + update_vec.begin() + POSITION_OFFSET + POSITION_SIZE); + int position_update_sum = std::accumulate(position_update_vec.begin(), + position_update_vec.end(), 0); + if (position_update_sum > 0) { + RCLCPP_WARN(this->get_logger(), + "Warning: Some position entries in parameter %s_config are listed " + "true, but sensor_msgs/Imu contains no information about position", + imu_topic_name); + } + std::vector linear_velocity_update_vec( + update_vec.begin() + POSITION_V_OFFSET, + update_vec.begin() + POSITION_V_OFFSET + LINEAR_VELOCITY_SIZE); + int linear_velocity_update_sum = std::accumulate( + linear_velocity_update_vec.begin(), linear_velocity_update_vec.end(), + 0); + if (linear_velocity_update_sum > 0) { + RCLCPP_WARN(this->get_logger(), + "Warning: Some linear velocity entries in parameter %s_config are " + "listed true, but an sensor_msgs/Imu contains no information about " + "linear velocities", imu_topic_name); + } + std::vector pose_update_vec = update_vec; + // IMU message contains no information about position, filter everything + // except orientation + std::fill(pose_update_vec.begin() + POSITION_OFFSET, + pose_update_vec.begin() + POSITION_OFFSET + POSITION_SIZE, 0); std::fill(pose_update_vec.begin() + POSITION_V_OFFSET, pose_update_vec.begin() + POSITION_V_OFFSET + TWIST_SIZE, 0); std::fill(pose_update_vec.begin() + POSITION_A_OFFSET, - pose_update_vec.begin() + POSITION_A_OFFSET + ACCELERATION_SIZE, - 0); + pose_update_vec.begin() + POSITION_A_OFFSET + ACCELERATION_SIZE, 0); std::vector twist_update_vec = update_vec; + // IMU message contains no information about linear speeds, filter + // everything except angular velocity std::fill(twist_update_vec.begin() + POSITION_OFFSET, twist_update_vec.begin() + POSITION_OFFSET + POSE_SIZE, 0); - std::fill( - twist_update_vec.begin() + POSITION_A_OFFSET, + std::fill(twist_update_vec.begin() + POSITION_V_OFFSET, + twist_update_vec.begin() + POSITION_V_OFFSET + LINEAR_VELOCITY_SIZE, 0); + std::fill(twist_update_vec.begin() + POSITION_A_OFFSET, twist_update_vec.begin() + POSITION_A_OFFSET + ACCELERATION_SIZE, 0); std::vector accel_update_vec = update_vec; @@ -1557,7 +1594,7 @@ void RosFilter::loadParams() if (abs_pose_var_counts[static_cast(state_var)] > 1) { std::stringstream stream; stream << abs_pose_var_counts[static_cast(state_var - - POSITION_OFFSET)] << " absolute pose inputs detected for " << + POSITION_OFFSET)] << " absolute pose inputs detected for " << state_variable_names_[state_var] << ". This may result in oscillations. Please ensure that your" "variances for each measured variable are set appropriately."; @@ -1910,13 +1947,13 @@ void RosFilter::periodicUpdate() tf2::fromMsg(world_base_link_trans_msg_.transform, world_base_link_trans); - tf2::Transform odom_base_link_trans; + tf2::Transform base_link_odom_trans; tf2::fromMsg(tf_buffer_ ->lookupTransform(base_link_frame_id_, odom_frame_id_, tf2::TimePointZero) .transform, - odom_base_link_trans); + base_link_odom_trans); /* * First, see these two references: @@ -1941,7 +1978,7 @@ void RosFilter::periodicUpdate() * transform. */ tf2::Transform map_odom_trans; - map_odom_trans.mult(world_base_link_trans, odom_base_link_trans); + map_odom_trans.mult(world_base_link_trans, base_link_odom_trans); geometry_msgs::msg::TransformStamped map_odom_trans_msg; map_odom_trans_msg.transform = tf2::toMsg(map_odom_trans); diff --git a/src/ros_robot_localization_listener.cpp b/src/ros_robot_localization_listener.cpp index f2f6ac249..29f97965b 100644 --- a/src/ros_robot_localization_listener.cpp +++ b/src/ros_robot_localization_listener.cpp @@ -382,7 +382,7 @@ bool RosRobotLocalizationListener::getState( ", so this doesn't make sense.", world_frame_id.c_str(), base_frame_id_.c_str()); return false; } - } catch (tf2::TransformException e) { + } catch (const tf2::TransformException & e) { RCLCPP_WARN(node_logger_->get_logger(), "Ros Robot Localization Listener: Could not look up transform: %s", e.what()); return false; @@ -413,7 +413,7 @@ bool RosRobotLocalizationListener::getState( "base frame: .", frame_id.c_str(), base_frame_id_.c_str()); return false; } - } catch (tf2::TransformException e) { + } catch (const tf2::TransformException & e) { RCLCPP_WARN(node_logger_->get_logger(), "Ros Robot Localization Listener: Could not look up transform: %s", e.what()); return false; diff --git a/src/ukf.cpp b/src/ukf.cpp index 624d58901..cc7da262a 100644 --- a/src/ukf.cpp +++ b/src/ukf.cpp @@ -292,6 +292,8 @@ void Ukf::predict( // We'll need these trig calculations a lot. double sp = ::sin(pitch); double cp = ::cos(pitch); + double cpi = 1.0 / cp; + double tp = sp * cpi; double sr = ::sin(roll); double cr = ::cos(roll); @@ -333,24 +335,13 @@ void Ukf::predict( 0.5 * transfer_function_(StateMemberZ, StateMemberVy) * delta_sec; transfer_function_(StateMemberZ, StateMemberAz) = 0.5 * transfer_function_(StateMemberZ, StateMemberVz) * delta_sec; - transfer_function_(StateMemberRoll, StateMemberVroll) = - transfer_function_(StateMemberX, StateMemberVx); - transfer_function_(StateMemberRoll, StateMemberVpitch) = - transfer_function_(StateMemberX, StateMemberVy); - transfer_function_(StateMemberRoll, StateMemberVyaw) = - transfer_function_(StateMemberX, StateMemberVz); - transfer_function_(StateMemberPitch, StateMemberVroll) = - transfer_function_(StateMemberY, StateMemberVx); - transfer_function_(StateMemberPitch, StateMemberVpitch) = - transfer_function_(StateMemberY, StateMemberVy); - transfer_function_(StateMemberPitch, StateMemberVyaw) = - transfer_function_(StateMemberY, StateMemberVz); - transfer_function_(StateMemberYaw, StateMemberVroll) = - transfer_function_(StateMemberZ, StateMemberVx); - transfer_function_(StateMemberYaw, StateMemberVpitch) = - transfer_function_(StateMemberZ, StateMemberVy); - transfer_function_(StateMemberYaw, StateMemberVyaw) = - transfer_function_(StateMemberZ, StateMemberVz); + transfer_function_(StateMemberRoll, StateMemberVroll) = delta_sec; + transfer_function_(StateMemberRoll, StateMemberVpitch) = sr * tp * delta_sec; + transfer_function_(StateMemberRoll, StateMemberVyaw) = cr * tp * delta_sec; + transfer_function_(StateMemberPitch, StateMemberVpitch) = cr * delta_sec; + transfer_function_(StateMemberPitch, StateMemberVyaw) = -sr * delta_sec; + transfer_function_(StateMemberYaw, StateMemberVpitch) = sr * cpi * delta_sec; + transfer_function_(StateMemberYaw, StateMemberVyaw) = cr * cpi * delta_sec; transfer_function_(StateMemberVx, StateMemberAx) = delta_sec; transfer_function_(StateMemberVy, StateMemberAy) = delta_sec; transfer_function_(StateMemberVz, StateMemberAz) = delta_sec; diff --git a/srv/FromLL.srv b/srv/FromLL.srv new file mode 100644 index 000000000..60c9d545a --- /dev/null +++ b/srv/FromLL.srv @@ -0,0 +1,3 @@ +geographic_msgs/GeoPoint ll_point +--- +geometry_msgs/Point map_point diff --git a/srv/ToLL.srv b/srv/ToLL.srv new file mode 100644 index 000000000..e99723d22 --- /dev/null +++ b/srv/ToLL.srv @@ -0,0 +1,3 @@ +geometry_msgs/Point map_point +--- +geographic_msgs/GeoPoint ll_point diff --git a/test/test_robot_localization_estimator.cpp b/test/test_robot_localization_estimator.cpp index ed2f851e7..eafb08053 100644 --- a/test/test_robot_localization_estimator.cpp +++ b/test/test_robot_localization_estimator.cpp @@ -92,7 +92,7 @@ TEST(RLETest, StateBuffer) // We filled the buffer with more states that it can hold, so its size should // now be equal to the capacity - EXPECT_EQ(static_cast(estimator.getSize()), buffer_capacity); + EXPECT_EQ(static_cast(estimator.getSize()), buffer_capacity); // Clear the buffer and check if it's really empty afterwards estimator.clearBuffer();