diff --git a/include/robot_localization/ros_filter.hpp b/include/robot_localization/ros_filter.hpp index e32ffeddc..7cc021dff 100644 --- a/include/robot_localization/ros_filter.hpp +++ b/include/robot_localization/ros_filter.hpp @@ -199,14 +199,14 @@ class RosFilter : public rclcpp::Node //! @param[out] message - The standard ROS odometry message to be filled //! @return true if the filter is initialized, false otherwise //! - bool getFilteredOdometryMessage(nav_msgs::msg::Odometry * message); + bool getFilteredOdometryMessage(nav_msgs::msg::Odometry & message); //! @brief Retrieves the EKF's acceleration output for broadcasting //! @param[out] message - The standard ROS acceleration message to be filled //! @return true if the filter is initialized, false otherwise //! bool getFilteredAccelMessage( - geometry_msgs::msg::AccelWithCovarianceStamped * message); + geometry_msgs::msg::AccelWithCovarianceStamped & message); //! @brief Callback method for receiving all IMU messages //! @param[in] msg - The ROS IMU message to take in. @@ -245,6 +245,31 @@ class RosFilter : public rclcpp::Node //! void periodicUpdate(); + //! @brief Update filter with data from measurements queue. + //! @param[in] time - The time at which to carry out integration. + //! + void updateFilterWithMeasurements(const rclcpp::Time & time); + + //! @brief publish world to base link transform and position + //! with odometry message + //! @param[in] filtered_position - filtered position from + //! getFilteredOdometryMessage. + //! @return true if data is corrected otherwise false. + //! + bool publishPositionWithOdometry(nav_msgs::msg::Odometry filtered_position); + + //! @brief Update world to base link transform using filtered position. + //! @param[in] filtered_position - filtered position from + //! getFilteredOdometryMessage. + //! + void updateWorldToBaseLinkTransform(const nav_msgs::msg::Odometry & filtered_position); + + //! @brief Publish world transform using frame id from filtered position. + //! @param[in] filtered_position - filtered position from + //! getFilteredOdometryMessage. + //! + void publishWorldTransform(const nav_msgs::msg::Odometry & filtered_position); + //! @brief Callback method for receiving all odometry messages //! @param[in] msg - The ROS odometry message to take in. //! @param[in] topic_name - The topic name for the odometry message (only used @@ -319,7 +344,7 @@ class RosFilter : public rclcpp::Node //! @param[out] message - The standard ROS odometry message to be validated //! @return true if the filter output is valid, false otherwise //! - bool validateFilterOutput(nav_msgs::msg::Odometry * message); + bool validateFilterOutput(nav_msgs::msg::Odometry & message); protected: //! @brief Finds the latest filter state before the given timestamp and makes diff --git a/src/ros_filter.cpp b/src/ros_filter.cpp index 4da645baf..b8d8c6439 100644 --- a/src/ros_filter.cpp +++ b/src/ros_filter.cpp @@ -364,7 +364,7 @@ void RosFilter::forceTwoD( } template -bool RosFilter::getFilteredOdometryMessage(nav_msgs::msg::Odometry * message) +bool RosFilter::getFilteredOdometryMessage(nav_msgs::msg::Odometry & message) { // If the filter has received a measurement at some point... if (filter_.getInitializedStatus()) { @@ -381,24 +381,24 @@ bool RosFilter::getFilteredOdometryMessage(nav_msgs::msg::Odometry * message) state(StateMemberYaw)); // Fill out the message - message->pose.pose.position.x = state(StateMemberX); - message->pose.pose.position.y = state(StateMemberY); - message->pose.pose.position.z = state(StateMemberZ); - message->pose.pose.orientation.x = quat.x(); - message->pose.pose.orientation.y = quat.y(); - message->pose.pose.orientation.z = quat.z(); - message->pose.pose.orientation.w = quat.w(); - message->twist.twist.linear.x = state(StateMemberVx); - message->twist.twist.linear.y = state(StateMemberVy); - message->twist.twist.linear.z = state(StateMemberVz); - message->twist.twist.angular.x = state(StateMemberVroll); - message->twist.twist.angular.y = state(StateMemberVpitch); - message->twist.twist.angular.z = state(StateMemberVyaw); + message.pose.pose.position.x = state(StateMemberX); + message.pose.pose.position.y = state(StateMemberY); + message.pose.pose.position.z = state(StateMemberZ); + message.pose.pose.orientation.x = quat.x(); + message.pose.pose.orientation.y = quat.y(); + message.pose.pose.orientation.z = quat.z(); + message.pose.pose.orientation.w = quat.w(); + message.twist.twist.linear.x = state(StateMemberVx); + message.twist.twist.linear.y = state(StateMemberVy); + message.twist.twist.linear.z = state(StateMemberVz); + message.twist.twist.angular.x = state(StateMemberVroll); + message.twist.twist.angular.y = state(StateMemberVpitch); + message.twist.twist.angular.z = state(StateMemberVyaw); // Our covariance matrix layout doesn't quite match for (size_t i = 0; i < POSE_SIZE; i++) { for (size_t j = 0; j < POSE_SIZE; j++) { - message->pose.covariance[POSE_SIZE * i + j] = + message.pose.covariance[POSE_SIZE * i + j] = estimate_error_covariance(i, j); } } @@ -408,16 +408,16 @@ bool RosFilter::getFilteredOdometryMessage(nav_msgs::msg::Odometry * message) // size of a pose covariance array for (size_t i = 0; i < TWIST_SIZE; i++) { for (size_t j = 0; j < TWIST_SIZE; j++) { - message->twist.covariance[TWIST_SIZE * i + j] = + message.twist.covariance[TWIST_SIZE * i + j] = estimate_error_covariance( i + POSITION_V_OFFSET, j + POSITION_V_OFFSET); } } - message->header.stamp = filter_.getLastMeasurementTime(); - message->header.frame_id = world_frame_id_; - message->child_frame_id = base_link_output_frame_id_; + message.header.stamp = filter_.getLastMeasurementTime(); + message.header.frame_id = world_frame_id_; + message.child_frame_id = base_link_output_frame_id_; } return filter_.getInitializedStatus(); @@ -425,7 +425,7 @@ bool RosFilter::getFilteredOdometryMessage(nav_msgs::msg::Odometry * message) template bool RosFilter::getFilteredAccelMessage( - geometry_msgs::msg::AccelWithCovarianceStamped * message) + geometry_msgs::msg::AccelWithCovarianceStamped & message) { // If the filter has received a measurement at some point... if (filter_.getInitializedStatus()) { @@ -435,23 +435,23 @@ bool RosFilter::getFilteredAccelMessage( filter_.getEstimateErrorCovariance(); //! Fill out the accel_msg - message->accel.accel.linear.x = state(StateMemberAx); - message->accel.accel.linear.y = state(StateMemberAy); - message->accel.accel.linear.z = state(StateMemberAz); + message.accel.accel.linear.x = state(StateMemberAx); + message.accel.accel.linear.y = state(StateMemberAy); + message.accel.accel.linear.z = state(StateMemberAz); // Fill the covariance (only the left-upper matrix since we are not // estimating the rotational accelerations arround the axes for (size_t i = 0; i < ACCELERATION_SIZE; i++) { for (size_t j = 0; j < ACCELERATION_SIZE; j++) { // We use the POSE_SIZE since the accel cov matrix of ROS is 6x6 - message->accel.covariance[POSE_SIZE * i + j] = estimate_error_covariance( + message.accel.covariance[POSE_SIZE * i + j] = estimate_error_covariance( i + POSITION_A_OFFSET, j + POSITION_A_OFFSET); } } // Fill header information - message->header.stamp = rclcpp::Time(filter_.getLastMeasurementTime()); - message->header.frame_id = base_link_output_frame_id_; + message.header.stamp = rclcpp::Time(filter_.getLastMeasurementTime()); + message.header.frame_id = base_link_output_frame_id_; } return filter_.getInitializedStatus(); @@ -2006,149 +2006,26 @@ void RosFilter::periodicUpdate() } rclcpp::Time cur_time = this->now(); - - if (toggled_on_) { - // Now we'll integrate any measurements we've received - integrateMeasurements(cur_time); - } else { - // Clear out measurements since we're not currently processing new entries - clearMeasurementQueue(); - - // Reset last measurement time so we don't get a large time delta on toggle - if (filter_.getInitializedStatus()) { - filter_.setLastMeasurementTime(this->now()); - } - } - - // Get latest state and publish it - auto filtered_position = std::make_unique(); - bool corrected_data = false; - if (getFilteredOdometryMessage(filtered_position.get())) { - world_base_link_trans_msg_.header.stamp = - static_cast(filtered_position->header.stamp) + tf_time_offset_; - world_base_link_trans_msg_.header.frame_id = - filtered_position->header.frame_id; - world_base_link_trans_msg_.child_frame_id = - filtered_position->child_frame_id; - - world_base_link_trans_msg_.transform.translation.x = - filtered_position->pose.pose.position.x; - world_base_link_trans_msg_.transform.translation.y = - filtered_position->pose.pose.position.y; - world_base_link_trans_msg_.transform.translation.z = - filtered_position->pose.pose.position.z; - world_base_link_trans_msg_.transform.rotation = - filtered_position->pose.pose.orientation; - - // The filtered_position is the message containing the state and covariances: - // nav_msgs Odometry - if (!validateFilterOutput(filtered_position.get())) { - RCLCPP_ERROR( - this->get_logger(), - "Critical Error, NaNs were detected in the output state of the filter. " - "This was likely due to poorly coniditioned process, noise, or sensor " - "covariances."); - } - - // If we're trying to publish with the same time stamp, it means that we had a measurement get - // inserted into the filter history, and our state estimate was updated after it was already - // published. As of ROS Noetic, TF2 will issue warnings whenever this occurs, so we make this - // behavior optional. Just for safety, we also check for the condition where the last published - // stamp is *later* than this stamp. This should never happen, but we should handle the case - // anyway. - corrected_data = (!permit_corrected_publication_ && - last_published_stamp_ >= filtered_position->header.stamp); - - // If the world_frame_id_ is the odom_frame_id_ frame, then we can just - // send the transform. If the world_frame_id_ is the map_frame_id_ frame, - // we'll have some work to do. - if (publish_transform_ && !corrected_data) { - if (filtered_position->header.frame_id == odom_frame_id_) { - world_transform_broadcaster_->sendTransform(world_base_link_trans_msg_); - } else if (filtered_position->header.frame_id == map_frame_id_) { - try { - tf2::Transform world_base_link_trans; - tf2::fromMsg( - world_base_link_trans_msg_.transform, - world_base_link_trans); - - tf2::Transform base_link_odom_trans; - tf2::fromMsg( - tf_buffer_ - ->lookupTransform( - base_link_frame_id_, - odom_frame_id_, - tf2::TimePointZero) - .transform, - base_link_odom_trans); - - /* - * First, see these two references: - * http://wiki.ros.org/tf/Overview/Using%20Published%20Transforms#lookupTransform - * http://wiki.ros.org/geometry/CoordinateFrameConventions#Transform_Direction - * We have a transform from map_frame_id_->base_link_frame_id_, but - * it would actually transform a given pose from - * base_link_frame_id_->map_frame_id_. We then used lookupTransform, - * whose first two arguments are target frame and source frame, to - * get a transform from base_link_frame_id_->odom_frame_id_. - * However, this transform would actually transform data from - * odom_frame_id_->base_link_frame_id_. Now imagine that we have a - * position in the map_frame_id_ frame. First, we multiply it by the - * inverse of the map_frame_id_->baseLinkFrameId, which will - * transform that data from map_frame_id_ to base_link_frame_id_. - * Now we want to go from base_link_frame_id_->odom_frame_id_, but - * the transform we have takes data from - * odom_frame_id_->base_link_frame_id_, so we need its inverse as - * well. We have now transformed our data from map_frame_id_ to - * odom_frame_id_. However, if we want other users to be able to do - * the same, we need to broadcast the inverse of that entire - * transform. - */ - tf2::Transform map_odom_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); - map_odom_trans_msg.header.stamp = - static_cast(filtered_position->header.stamp) + tf_time_offset_; - map_odom_trans_msg.header.frame_id = map_frame_id_; - map_odom_trans_msg.child_frame_id = odom_frame_id_; - - world_transform_broadcaster_->sendTransform(map_odom_trans_msg); - } catch (...) { - // ROS_ERROR_STREAM_DELAYED_THROTTLE(5.0, "Could not obtain - // transform from " - // << odom_frame_id_ << "->" << - // base_link_frame_id_); - } - } else { - std::cerr << "Odometry message frame_id was " << - filtered_position->header.frame_id << ", expected " << - map_frame_id_ << " or " << odom_frame_id_ << "\n"; - } - } - - // Retain the last published stamp so we can detect repeated transforms in future cycles - last_published_stamp_ = filtered_position->header.stamp; - - // Fire off the position and the transform - if (!corrected_data) { - position_pub_->publish(std::move(filtered_position)); - } + updateFilterWithMeasurements(cur_time); - if (print_diagnostics_) { - freq_diag_->tick(); + // Get latest state and publish it + { + nav_msgs::msg::Odometry filtered_position; + if (getFilteredOdometryMessage(filtered_position)) { + corrected_data = publishPositionWithOdometry(std::move(filtered_position)); } } // Publish the acceleration if desired and filter is initialized - auto filtered_acceleration = std::make_unique(); - if (!corrected_data && publish_acceleration_ && - getFilteredAccelMessage(filtered_acceleration.get())) { - accel_pub_->publish(std::move(filtered_acceleration)); + geometry_msgs::msg::AccelWithCovarianceStamped filtered_acceleration; + if (!corrected_data && publish_acceleration_ && + getFilteredAccelMessage(filtered_acceleration)) + { + accel_pub_->publish(std::move(filtered_acceleration)); + } } /* Diagnostics can behave strangely when playing back from bag @@ -2180,6 +2057,164 @@ void RosFilter::periodicUpdate() } } +template +void RosFilter::updateFilterWithMeasurements(const rclcpp::Time & time) +{ + if (toggled_on_) { + // Now we'll integrate any measurements we've received + integrateMeasurements(time); + } else { + // Clear out measurements since we're not currently processing new entries + clearMeasurementQueue(); + + // Reset last measurement time so we don't get a large time delta on toggle + if (filter_.getInitializedStatus()) { + filter_.setLastMeasurementTime(time); + } + } +} + +template +bool RosFilter::publishPositionWithOdometry(nav_msgs::msg::Odometry filtered_position) +{ + updateWorldToBaseLinkTransform(filtered_position); + + // The filtered_position is the message containing the state and covariances: + // nav_msgs Odometry + if (!validateFilterOutput(filtered_position)) { + RCLCPP_ERROR( + this->get_logger(), + "Critical Error, NaNs were detected in the output state of the filter. " + "This was likely due to poorly conditioned process, noise, or sensor " + "covariances."); + } + + // If we're trying to publish with the same time stamp, it means that we had a measurement get + // inserted into the filter history, and our state estimate was updated after it was already + // published. As of ROS Noetic, TF2 will issue warnings whenever this occurs, so we make this + // behavior optional. Just for safety, we also check for the condition where the last published + // stamp is *later* than this stamp. This should never happen, but we should handle the case + // anyway. + bool corrected_data = (!permit_corrected_publication_ && + last_published_stamp_ >= filtered_position.header.stamp); + + if (publish_transform_ && !corrected_data) { + publishWorldTransform(filtered_position); + } + + // Retain the last published stamp so we can detect repeated transforms in future cycles + last_published_stamp_ = filtered_position.header.stamp; + + // Fire off the position and the transform + if (!corrected_data) { + position_pub_->publish(std::move(filtered_position)); + } + + if (print_diagnostics_) { + freq_diag_->tick(); + } + + return corrected_data; +} + +template +void RosFilter::updateWorldToBaseLinkTransform(const nav_msgs::msg::Odometry & filtered_position) +{ + world_base_link_trans_msg_.header.stamp = + static_cast(filtered_position.header.stamp) + tf_time_offset_; + world_base_link_trans_msg_.header.frame_id = + filtered_position.header.frame_id; + world_base_link_trans_msg_.child_frame_id = + filtered_position.child_frame_id; + + world_base_link_trans_msg_.transform.translation.x = + filtered_position.pose.pose.position.x; + world_base_link_trans_msg_.transform.translation.y = + filtered_position.pose.pose.position.y; + world_base_link_trans_msg_.transform.translation.z = + filtered_position.pose.pose.position.z; + world_base_link_trans_msg_.transform.rotation = + filtered_position.pose.pose.orientation; +} + +template +void RosFilter::publishWorldTransform(const nav_msgs::msg::Odometry & filtered_position) +{ + // If the world_frame_id_ is the odom_frame_id_ frame, then we can just + // send the transform. If the world_frame_id_ is the map_frame_id_ frame, + // we'll have some work to do. + + auto frame_id = filtered_position.header.frame_id; + + if (frame_id == odom_frame_id_) { + world_transform_broadcaster_->sendTransform(world_base_link_trans_msg_); + return; + } + + if (frame_id != map_frame_id_) { + std::cerr << "Odometry message frame_id was " << + frame_id << ", expected " << + map_frame_id_ << " or " << odom_frame_id_ << "\n"; + return; + } + + try { + tf2::Transform world_base_link_trans; + tf2::fromMsg( + world_base_link_trans_msg_.transform, + world_base_link_trans); + + tf2::Transform base_link_odom_trans; + tf2::fromMsg( + tf_buffer_ + ->lookupTransform( + base_link_frame_id_, + odom_frame_id_, + tf2::TimePointZero) + .transform, + base_link_odom_trans); + + /* + * First, see these two references: + * http://wiki.ros.org/tf/Overview/Using%20Published%20Transforms#lookupTransform + * http://wiki.ros.org/geometry/CoordinateFrameConventions#Transform_Direction + * We have a transform from map_frame_id_->base_link_frame_id_, but + * it would actually transform a given pose from + * base_link_frame_id_->map_frame_id_. We then used lookupTransform, + * whose first two arguments are target frame and source frame, to + * get a transform from base_link_frame_id_->odom_frame_id_. + * However, this transform would actually transform data from + * odom_frame_id_->base_link_frame_id_. Now imagine that we have a + * position in the map_frame_id_ frame. First, we multiply it by the + * inverse of the map_frame_id_->baseLinkFrameId, which will + * transform that data from map_frame_id_ to base_link_frame_id_. + * Now we want to go from base_link_frame_id_->odom_frame_id_, but + * the transform we have takes data from + * odom_frame_id_->base_link_frame_id_, so we need its inverse as + * well. We have now transformed our data from map_frame_id_ to + * odom_frame_id_. However, if we want other users to be able to do + * the same, we need to broadcast the inverse of that entire + * transform. + */ + tf2::Transform map_odom_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); + map_odom_trans_msg.header.stamp = + static_cast(filtered_position.header.stamp) + tf_time_offset_; + map_odom_trans_msg.header.frame_id = map_frame_id_; + map_odom_trans_msg.child_frame_id = odom_frame_id_; + + world_transform_broadcaster_->sendTransform(map_odom_trans_msg); + } catch (...) { + // ROS_ERROR_STREAM_DELAYED_THROTTLE(5.0, "Could not obtain + // transform from " + // << odom_frame_id_ << "->" << + // base_link_frame_id_); + } +} + template void RosFilter::setPoseCallback( const geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr msg) @@ -3383,34 +3418,34 @@ bool RosFilter::revertTo(const rclcpp::Time & time) } template -bool RosFilter::validateFilterOutput(nav_msgs::msg::Odometry * message) +bool RosFilter::validateFilterOutput(nav_msgs::msg::Odometry & message) { - return !std::isnan(message->pose.pose.position.x) && - !std::isinf(message->pose.pose.position.x) && - !std::isnan(message->pose.pose.position.y) && - !std::isinf(message->pose.pose.position.y) && - !std::isnan(message->pose.pose.position.z) && - !std::isinf(message->pose.pose.position.z) && - !std::isnan(message->pose.pose.orientation.x) && - !std::isinf(message->pose.pose.orientation.x) && - !std::isnan(message->pose.pose.orientation.y) && - !std::isinf(message->pose.pose.orientation.y) && - !std::isnan(message->pose.pose.orientation.z) && - !std::isinf(message->pose.pose.orientation.z) && - !std::isnan(message->pose.pose.orientation.w) && - !std::isinf(message->pose.pose.orientation.w) && - !std::isnan(message->twist.twist.linear.x) && - !std::isinf(message->twist.twist.linear.x) && - !std::isnan(message->twist.twist.linear.y) && - !std::isinf(message->twist.twist.linear.y) && - !std::isnan(message->twist.twist.linear.z) && - !std::isinf(message->twist.twist.linear.z) && - !std::isnan(message->twist.twist.angular.x) && - !std::isinf(message->twist.twist.angular.x) && - !std::isnan(message->twist.twist.angular.y) && - !std::isinf(message->twist.twist.angular.y) && - !std::isnan(message->twist.twist.angular.z) && - !std::isinf(message->twist.twist.angular.z); + return !std::isnan(message.pose.pose.position.x) && + !std::isinf(message.pose.pose.position.x) && + !std::isnan(message.pose.pose.position.y) && + !std::isinf(message.pose.pose.position.y) && + !std::isnan(message.pose.pose.position.z) && + !std::isinf(message.pose.pose.position.z) && + !std::isnan(message.pose.pose.orientation.x) && + !std::isinf(message.pose.pose.orientation.x) && + !std::isnan(message.pose.pose.orientation.y) && + !std::isinf(message.pose.pose.orientation.y) && + !std::isnan(message.pose.pose.orientation.z) && + !std::isinf(message.pose.pose.orientation.z) && + !std::isnan(message.pose.pose.orientation.w) && + !std::isinf(message.pose.pose.orientation.w) && + !std::isnan(message.twist.twist.linear.x) && + !std::isinf(message.twist.twist.linear.x) && + !std::isnan(message.twist.twist.linear.y) && + !std::isinf(message.twist.twist.linear.y) && + !std::isnan(message.twist.twist.linear.z) && + !std::isinf(message.twist.twist.linear.z) && + !std::isnan(message.twist.twist.angular.x) && + !std::isinf(message.twist.twist.angular.x) && + !std::isnan(message.twist.twist.angular.y) && + !std::isinf(message.twist.twist.angular.y) && + !std::isnan(message.twist.twist.angular.z) && + !std::isinf(message.twist.twist.angular.z); } template