From ecfffe5be91f5c4020f6dca42f4e358379bfb70b Mon Sep 17 00:00:00 2001 From: robbiefish Date: Thu, 25 Jul 2024 18:21:15 +0000 Subject: [PATCH 1/2] Adds ability to use or not use the ROS vehicle frame --- config/params.yml | 16 +- .../config.h | 21 +- src/config.cpp | 79 ++-- src/publishers.cpp | 354 +++++++----------- src/subscribers.cpp | 47 +-- 5 files changed, 220 insertions(+), 297 deletions(-) diff --git a/config/params.yml b/config/params.yml index 8aa9669..f7f488a 100644 --- a/config/params.yml +++ b/config/params.yml @@ -112,10 +112,16 @@ mount_to_frame_id_transform : [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0] # [ x # true - position, velocity, and orientation are WRT the ENU frame # # Note: It is strongly recommended to leave this as True if you plan to integrate with other ROS tools -# Note: If this is set to false, all "*_frame_id" variables will have "_ned" appended to them by the driver. # For more information, see: https://wiki.ros.org/microstrain_inertial_driver/use_enu_frame use_enu_frame : True +# Controls if the driver outputs local level data in the ROS vehicle frame +# false - position, velocity, and orientation are in the Microstrain vehicle frame (native device frame) +# true - position, velocity, and orientation are in the ROS vehicle frame +# +# Note: If this is set to false, and the device is sitting flat and level, it will appear that the device is upside down +# For more information, see: https://wiki.ros.org/microstrain_inertial_driver/use_ros_vehicle_frame +use_ros_vehicle_frame : True # ****************************************************************** # GNSS Settings (only applicable for devices with GNSS) @@ -195,7 +201,7 @@ filter_enable_external_heading_aiding : False # Reference frame = # 1 - WGS84 Earth-fixed, earth centered (ECEF) position, velocity, attitude # 2 - WGS84 Latitude, Longitude, height above ellipsoid position, NED velocity and attitude -filter_init_condition_src : 0 +filter_init_condition_src : 3 filter_auto_heading_alignment_selector : 1 filter_init_reference_frame : 2 filter_init_position : [0.0, 0.0, 0.0] @@ -263,8 +269,10 @@ filter_pps_source : 1 # The different options are provided as a convenience. # Support for matrix and quaternion options is firmware version dependent (GQ7 supports Quaternion as of firmware 1.0.07) # Quaternion order is [i, j, k, w] -# Note: This can cause strange behavior when also using the ROS transform tree. -# It is recommended to not use this if you want to use the ROS transform tree unless you really know what you are doing +# Note: GNSS antenna offsets are measured WRT to the 'vehicle' frame. If you change this transform, make sure your antenna offsets are measured properly. +# See https://files.microstrain.com/GQ7+User+Manual/user_manual_content/installation/Antenna.htm for more information. +# Note: This transform does not take into account any transforms that may be in the ROS tf tree. +# If you attempt to use both, downstream consumers of the data that use the ROS tf tree will apply the same transform twice. filter_sensor2vehicle_frame_selector : 1 filter_sensor2vehicle_frame_transformation_euler : [0.0, 0.0, 0.0] filter_sensor2vehicle_frame_transformation_matrix : [1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0] diff --git a/include/microstrain_inertial_driver_common/config.h b/include/microstrain_inertial_driver_common/config.h index b2638d6..3769d2a 100644 --- a/include/microstrain_inertial_driver_common/config.h +++ b/include/microstrain_inertial_driver_common/config.h @@ -89,11 +89,26 @@ class Config int reconnect_attempts_; bool configure_after_reconnect_; - // Info for converting to the ENU frame + // Whether we will use the ENU or NED frame for global frame data bool use_enu_frame_; - tf2::Transform ned_to_enu_transform_tf_; - tf2::Transform ros_vehicle_to_microstrain_vehicle_transform_tf_; + // Whether we will use the ROS vehicle or Microstrain vehicle frame for local frame data + bool use_ros_vehicle_frame_; + + // Transform used to transform from the microstrain global frame to the driver global frame. + // If the driver is configured with use_enu_frame = true, this will transform NED -> ENU + // If the driver is configured with use_enu_frame = false, this will transform NED -> NED (identity transform) + tf2::Transform microstrain_global_frame_to_driver_global_frame_transform_tf_; + tf2::Transform driver_global_frame_to_microstrain_global_frame_transform_tf_; // This will just be the inverse of the above transform + + // Transform used to transform from the microstrain vehicle frame to the driver vehicle frame. + // If the driver is configured with use_ros_vehicle_frame = true, this will transform Microstrain Vehicle Frame -> ROS Vehicle Frame + // If the driver is configured with use_ros_vehicle_frame = false, this will transform Microstrain Vehicle Frame -> Microstrain Vehicle Frame (identity transform) + tf2::Transform microstrain_vehicle_frame_to_driver_vehicle_frame_transform_tf_; + tf2::Transform driver_vehicle_frame_to_microstrain_vehicle_frame_transform_tf_; // This will just be the inverse of the above transform + + //tf2::Transform ned_to_enu_transform_tf_; + //tf2::Transform ros_vehicle_to_microstrain_vehicle_transform_tf_; // Whether to enable the hardware odometer through the GPIO pins bool enable_hardware_odometer_; diff --git a/src/config.cpp b/src/config.cpp index 44ef3b8..d6b7584 100644 --- a/src/config.cpp +++ b/src/config.cpp @@ -44,18 +44,6 @@ Config::Config(RosNodeType* node) : node_(node) bool Config::configure(RosNodeType* node) { - // Initialize some default and static config - ned_to_enu_transform_tf_ = tf2::Transform(tf2::Matrix3x3( - 0, 1, 0, - 1, 0, 0, - 0, 0, -1 - )); - ros_vehicle_to_microstrain_vehicle_transform_tf_ = tf2::Transform(tf2::Matrix3x3( - 1, 0, 0, - 0, -1, 0, - 0, 0, -1 - )); - /// /// Generic configuration used by the rest of the driver /// @@ -77,18 +65,43 @@ bool Config::configure(RosNodeType* node) getParam(node, "gnss1_frame_id", gnss_frame_id_[GNSS1_ID], "gnss_1_antenna_link"); getParam(node, "gnss2_frame_id", gnss_frame_id_[GNSS2_ID], "gnss_2_antenna_link"); getParam(node, "odometer_frame_id", odometer_frame_id_, "odometer_link"); - getParam(node, "use_enu_frame", use_enu_frame_, false); - // tf config - getParam(node, "tf_mode", tf_mode_, TF_MODE_GLOBAL); - getParam(node, "publish_mount_to_frame_id_transform", publish_mount_to_frame_id_transform_, true); + // Driver frame configuration + getParam(node, "use_enu_frame", use_enu_frame_, true); + getParam(node, "use_ros_vehicle_frame", use_ros_vehicle_frame_, true); + + // The definition of the "driver" world frame and vehicle frame differs depending on use_enu_frame and use_ros_vehicle_frame + if (use_enu_frame_) + { + microstrain_global_frame_to_driver_global_frame_transform_tf_ = tf2::Transform(tf2::Matrix3x3( + 0, 1, 0, + 1, 0, 0, + 0, 0, -1 + )); + } + else + { + microstrain_global_frame_to_driver_global_frame_transform_tf_ = tf2::Transform::getIdentity(); + } + driver_global_frame_to_microstrain_global_frame_transform_tf_ = microstrain_global_frame_to_driver_global_frame_transform_tf_.inverse(); - // If using the NED frame, append that to the map frame ID - if (!use_enu_frame_) + if (use_ros_vehicle_frame_) + { + microstrain_vehicle_frame_to_driver_vehicle_frame_transform_tf_ = tf2::Transform(tf2::Matrix3x3( + 1, 0, 0, + 0, -1, 0, + 0, 0, -1 + )); + } + else { - constexpr char ned_suffix[] = "_ned"; - map_frame_id_ += ned_suffix; + microstrain_vehicle_frame_to_driver_vehicle_frame_transform_tf_ = tf2::Transform::getIdentity(); } + driver_vehicle_frame_to_microstrain_vehicle_frame_transform_tf_ = microstrain_vehicle_frame_to_driver_vehicle_frame_transform_tf_.inverse(); + + // tf config + getParam(node, "tf_mode", tf_mode_, TF_MODE_GLOBAL); + getParam(node, "publish_mount_to_frame_id_transform", publish_mount_to_frame_id_transform_, true); // Configure the static transforms std::vector mount_to_frame_id_transform_vec; @@ -690,27 +703,15 @@ bool Config::configureFilter(RosNodeType* node) MICROSTRAIN_WARN(node_, "Timed out waiting for transform from %s to %s, tf error: %s", frame_id_.c_str(), gnss_frame_id_[i].c_str(), tf_error_string.c_str()); } - // If not using the enu frame, this can be plugged directly into the device, otherwise rotate it from the ROS body frame to our body frame - TransformStampedMsg gnss_antenna_to_microstrain_vehicle_transform; - if (use_enu_frame_) - { - const auto& gnss_antenna_to_ros_vehicle_transform = transform_buffer_->lookupTransform(frame_id_, gnss_frame_id_[i], frame_time); - - tf2::Transform gnss_antenna_to_ros_vehicle_transform_tf; - tf2::fromMsg(gnss_antenna_to_ros_vehicle_transform.transform, gnss_antenna_to_ros_vehicle_transform_tf); - - const auto& gnss_antenna_to_microstrain_vehicle_transform_tf = ros_vehicle_to_microstrain_vehicle_transform_tf_ * gnss_antenna_to_ros_vehicle_transform_tf; - gnss_antenna_to_microstrain_vehicle_transform.transform = tf2::toMsg(gnss_antenna_to_microstrain_vehicle_transform_tf); - } - else - { - gnss_antenna_to_microstrain_vehicle_transform = transform_buffer_->lookupTransform(frame_id_, gnss_frame_id_[i], frame_time); - } + // Fetch the transform and modify it to the correct frame. If using the ROS vehicle frame, assume it is in the ROS vehicle frame, otherwise assume it is in the MicroStrain vehicle frame + const auto& gnss_antenna_to_driver_vehicle_frame_transform = transform_buffer_->lookupTransform(frame_id_, gnss_frame_id_[i], frame_time); + const tf2::Vector3 gnss_antenna_to_driver_vehicle_frame_translation_tf(gnss_antenna_to_driver_vehicle_frame_transform.transform.translation.x, gnss_antenna_to_driver_vehicle_frame_transform.transform.translation.y, gnss_antenna_to_driver_vehicle_frame_transform.transform.translation.z); + const tf2::Vector3& gnss_antenna_to_microstrain_vehicle_transform_tf = driver_vehicle_frame_to_microstrain_vehicle_frame_transform_tf_ * gnss_antenna_to_driver_vehicle_frame_translation_tf; // Override the antenna offset with the result from the transform tree - gnss_antenna_offset_[i][0] = gnss_antenna_to_microstrain_vehicle_transform.transform.translation.x; - gnss_antenna_offset_[i][1] = gnss_antenna_to_microstrain_vehicle_transform.transform.translation.y; - gnss_antenna_offset_[i][2] = gnss_antenna_to_microstrain_vehicle_transform.transform.translation.z; + gnss_antenna_offset_[i][0] = gnss_antenna_to_microstrain_vehicle_transform_tf.getX(); + gnss_antenna_offset_[i][1] = gnss_antenna_to_microstrain_vehicle_transform_tf.getY(); + gnss_antenna_offset_[i][2] = gnss_antenna_to_microstrain_vehicle_transform_tf.getZ(); } } diff --git a/src/publishers.cpp b/src/publishers.cpp index ff537db..2d98982 100644 --- a/src/publishers.cpp +++ b/src/publishers.cpp @@ -225,20 +225,12 @@ bool Publishers::configure() gnss_1_antenna_link_to_imu_link_transform.child_frame_id = config_->gnss_frame_id_[GNSS1_ID]; gnss_1_antenna_link_to_imu_link_transform.transform.rotation = tf2::toMsg(tf2::Quaternion::getIdentity()); - const tf2::Transform gnss_1_antenna_link_to_microstrain_vehicle_transform_tf(tf2::Quaternion::getIdentity(), tf2::Vector3(gnss_antenna_offsets[0], gnss_antenna_offsets[1], gnss_antenna_offsets[2])); - if (config_->use_enu_frame_) - { - const tf2::Transform gnss_1_antenna_link_to_ros_vehicle_transform_tf = config_->ros_vehicle_to_microstrain_vehicle_transform_tf_.inverse() * gnss_1_antenna_link_to_microstrain_vehicle_transform_tf; - gnss_1_antenna_link_to_imu_link_transform.transform.translation.x = gnss_1_antenna_link_to_ros_vehicle_transform_tf.getOrigin().getX(); - gnss_1_antenna_link_to_imu_link_transform.transform.translation.y = gnss_1_antenna_link_to_ros_vehicle_transform_tf.getOrigin().getY(); - gnss_1_antenna_link_to_imu_link_transform.transform.translation.z = gnss_1_antenna_link_to_ros_vehicle_transform_tf.getOrigin().getZ(); - } - else - { - gnss_1_antenna_link_to_imu_link_transform.transform.translation.x = gnss_1_antenna_link_to_microstrain_vehicle_transform_tf.getOrigin().getX(); - gnss_1_antenna_link_to_imu_link_transform.transform.translation.y = gnss_1_antenna_link_to_microstrain_vehicle_transform_tf.getOrigin().getY(); - gnss_1_antenna_link_to_imu_link_transform.transform.translation.z = gnss_1_antenna_link_to_microstrain_vehicle_transform_tf.getOrigin().getZ(); - } + const tf2::Vector3 gnss_1_antenna_link_to_microstrain_vehicle_translation_tf(gnss_antenna_offsets[0], gnss_antenna_offsets[1], gnss_antenna_offsets[2]); + const tf2::Vector3 gnss_1_antenna_link_to_driver_vehicle_translation_tf = config_->microstrain_vehicle_frame_to_driver_vehicle_frame_transform_tf_ * gnss_1_antenna_link_to_driver_vehicle_translation_tf; + + gnss_1_antenna_link_to_imu_link_transform.transform.translation.x = gnss_1_antenna_link_to_driver_vehicle_translation_tf.getX(); + gnss_1_antenna_link_to_imu_link_transform.transform.translation.y = gnss_1_antenna_link_to_driver_vehicle_translation_tf.getY(); + gnss_1_antenna_link_to_imu_link_transform.transform.translation.z = gnss_1_antenna_link_to_driver_vehicle_translation_tf.getZ(); } else if (config_->mip_device_->supportsDescriptor(mip::commands_filter::DESCRIPTOR_SET, mip::commands_filter::CMD_MULTI_ANTENNA_OFFSET)) { @@ -255,20 +247,12 @@ bool Publishers::configure() gnss_x_antenna_link_to_imu_link_transform.child_frame_id = config_->gnss_frame_id_[gnss_id]; gnss_x_antenna_link_to_imu_link_transform.transform.rotation = tf2::toMsg(tf2::Quaternion::getIdentity()); - const tf2::Transform gnss_x_antenna_link_to_microstrain_vehicle_transform_tf(tf2::Quaternion::getIdentity(), tf2::Vector3(gnss_antenna_offsets[0], gnss_antenna_offsets[1], gnss_antenna_offsets[2])); - if (config_->use_enu_frame_) - { - const tf2::Transform gnss_x_antenna_link_to_ros_vehicle_transform_tf = config_->ros_vehicle_to_microstrain_vehicle_transform_tf_.inverse() * gnss_x_antenna_link_to_microstrain_vehicle_transform_tf; - gnss_x_antenna_link_to_imu_link_transform.transform.translation.x = gnss_x_antenna_link_to_ros_vehicle_transform_tf.getOrigin().getX(); - gnss_x_antenna_link_to_imu_link_transform.transform.translation.y = gnss_x_antenna_link_to_ros_vehicle_transform_tf.getOrigin().getY(); - gnss_x_antenna_link_to_imu_link_transform.transform.translation.z = gnss_x_antenna_link_to_ros_vehicle_transform_tf.getOrigin().getZ(); - } - else - { - gnss_x_antenna_link_to_imu_link_transform.transform.translation.x = gnss_x_antenna_link_to_microstrain_vehicle_transform_tf.getOrigin().getX(); - gnss_x_antenna_link_to_imu_link_transform.transform.translation.y = gnss_x_antenna_link_to_microstrain_vehicle_transform_tf.getOrigin().getY(); - gnss_x_antenna_link_to_imu_link_transform.transform.translation.z = gnss_x_antenna_link_to_microstrain_vehicle_transform_tf.getOrigin().getZ(); - } + const tf2::Vector3 gnss_x_antenna_link_to_microstrain_vehicle_translation_tf(gnss_antenna_offsets[0], gnss_antenna_offsets[1], gnss_antenna_offsets[2]); + const tf2::Vector3 gnss_x_antenna_link_to_driver_vehicle_translation_tf = config_->microstrain_vehicle_frame_to_driver_vehicle_frame_transform_tf_ * gnss_x_antenna_link_to_driver_vehicle_translation_tf; + + gnss_x_antenna_link_to_imu_link_transform.transform.translation.x = gnss_x_antenna_link_to_driver_vehicle_translation_tf.getX(); + gnss_x_antenna_link_to_imu_link_transform.transform.translation.y = gnss_x_antenna_link_to_driver_vehicle_translation_tf.getY(); + gnss_x_antenna_link_to_imu_link_transform.transform.translation.z = gnss_x_antenna_link_to_driver_vehicle_translation_tf.getZ(); } } @@ -288,20 +272,11 @@ bool Publishers::configure() odometer_link_to_imu_link_transform_.transform.rotation = tf2::toMsg(tf2::Quaternion::getIdentity()); // If running in the ENU frame, transform from our vehicle frame to the ROS vehicle frame - const tf2::Transform odometer_link_to_microstrain_vehicle_transform_tf(tf2::Quaternion::getIdentity(), tf2::Vector3(speed_lever_arm[0], speed_lever_arm[1], speed_lever_arm[2])); - if (config_->use_enu_frame_) - { - const tf2::Transform odometer_link_to_ros_vehicle_transform_tf = config_->ros_vehicle_to_microstrain_vehicle_transform_tf_.inverse() * odometer_link_to_microstrain_vehicle_transform_tf; - odometer_link_to_imu_link_transform_.transform.translation.x = odometer_link_to_ros_vehicle_transform_tf.getOrigin().getX(); - odometer_link_to_imu_link_transform_.transform.translation.y = odometer_link_to_ros_vehicle_transform_tf.getOrigin().getY(); - odometer_link_to_imu_link_transform_.transform.translation.z = odometer_link_to_ros_vehicle_transform_tf.getOrigin().getZ(); - } - else - { - odometer_link_to_imu_link_transform_.transform.translation.x = odometer_link_to_microstrain_vehicle_transform_tf.getOrigin().getX(); - odometer_link_to_imu_link_transform_.transform.translation.y = odometer_link_to_microstrain_vehicle_transform_tf.getOrigin().getY(); - odometer_link_to_imu_link_transform_.transform.translation.z = odometer_link_to_microstrain_vehicle_transform_tf.getOrigin().getZ(); - } + const tf2::Vector3 odometer_link_to_microstrain_vehicle_translation_tf(speed_lever_arm[0], speed_lever_arm[1], speed_lever_arm[2]); + const tf2::Vector3 odometer_link_to_driver_vehicle_translation_tf = config_->microstrain_vehicle_frame_to_driver_vehicle_frame_transform_tf_ * odometer_link_to_driver_vehicle_translation_tf; + odometer_link_to_imu_link_transform_.transform.translation.x = odometer_link_to_driver_vehicle_translation_tf.getX(); + odometer_link_to_imu_link_transform_.transform.translation.y = odometer_link_to_driver_vehicle_translation_tf.getY(); + odometer_link_to_imu_link_transform_.transform.translation.z = odometer_link_to_driver_vehicle_translation_tf.getZ(); } // Register callbacks for each data field we care about. Note that order is preserved here, so if a data field needs to be parsed before another, change it here. @@ -424,12 +399,20 @@ bool Publishers::activate() // Static antenna offsets // Note: If streaming the antenna offset correction topic, correct the offsets with them - if (config_->gnss_antenna_offset_source_[GNSS1_ID] == GNSS_ANTENNA_OFFSET_SOURCE_MANUAL) - if (config_->mip_device_->supportsDescriptorSet(mip::data_gnss::DESCRIPTOR_SET) || config_->mip_device_->supportsDescriptorSet(mip::data_gnss::MIP_GNSS1_DATA_DESC_SET)) + if (config_->gnss_antenna_offset_source_[GNSS1_ID] == GNSS_ANTENNA_OFFSET_SOURCE_MANUAL && (config_->mip_device_->supportsDescriptorSet(mip::data_gnss::DESCRIPTOR_SET) || config_->mip_device_->supportsDescriptorSet(mip::data_gnss::MIP_GNSS1_DATA_DESC_SET))) + { + if (mip_filter_multi_antenna_offset_correction_pub_->dataRate() > 0) + transform_broadcaster_->sendTransform(gnss_antenna_link_to_imu_link_transform_[GNSS1_ID]); + else static_transform_broadcaster_->sendTransform(gnss_antenna_link_to_imu_link_transform_[GNSS1_ID]); - if (config_->gnss_antenna_offset_source_[GNSS2_ID] == GNSS_ANTENNA_OFFSET_SOURCE_MANUAL) - if (config_->mip_device_->supportsDescriptorSet(mip::data_gnss::MIP_GNSS2_DATA_DESC_SET)) + } + if (config_->gnss_antenna_offset_source_[GNSS2_ID] == GNSS_ANTENNA_OFFSET_SOURCE_MANUAL && config_->mip_device_->supportsDescriptorSet(mip::data_gnss::MIP_GNSS2_DATA_DESC_SET)) + { + if (mip_filter_multi_antenna_offset_correction_pub_->dataRate() > 0) + transform_broadcaster_->sendTransform(gnss_antenna_link_to_imu_link_transform_[GNSS2_ID]); + else static_transform_broadcaster_->sendTransform(gnss_antenna_link_to_imu_link_transform_[GNSS2_ID]); + } if (config_->mip_device_->supportsDescriptor(mip::commands_filter::DESCRIPTOR_SET, mip::commands_filter::CMD_SPEED_LEVER_ARM)) static_transform_broadcaster_->sendTransform(odometer_link_to_imu_link_transform_); return true; @@ -641,14 +624,15 @@ void Publishers::handleSensorDeltaTheta(const mip::data_sensor::DeltaTheta& delt // We use this delta measurement to exclude outliers and provide a more useful IMU measurement auto imu_msg = imu_pub_->getMessageToUpdate(); updateHeaderTime(&(imu_msg->header), descriptor_set, timestamp); - imu_msg->angular_velocity.x = delta_theta.delta_theta[0] / delta_time; - imu_msg->angular_velocity.y = delta_theta.delta_theta[1] / delta_time; - imu_msg->angular_velocity.z = delta_theta.delta_theta[2] / delta_time; - if (config_->use_enu_frame_) - { - imu_msg->angular_velocity.y *= -1.0; - imu_msg->angular_velocity.z *= -1.0; - } + const tf2::Vector3 angular_velocity_in_microstrain_vehicle_frame( + delta_theta.delta_theta[0] / delta_time, + delta_theta.delta_theta[1] / delta_time, + delta_theta.delta_theta[2] / delta_time + ); + const tf2::Vector3& angular_velocity_in_driver_vehicle_frame = config_->microstrain_vehicle_frame_to_driver_vehicle_frame_transform_tf_ * angular_velocity_in_microstrain_vehicle_frame; + imu_msg->angular_velocity.x = angular_velocity_in_driver_vehicle_frame.getX(); + imu_msg->angular_velocity.y = angular_velocity_in_driver_vehicle_frame.getY(); + imu_msg->angular_velocity.z = angular_velocity_in_driver_vehicle_frame.getZ(); } void Publishers::handleSensorDeltaVelocity(const mip::data_sensor::DeltaVelocity& delta_velocity, const uint8_t descriptor_set, mip::Timestamp timestamp) @@ -662,14 +646,15 @@ void Publishers::handleSensorDeltaVelocity(const mip::data_sensor::DeltaVelocity auto imu_msg = imu_pub_->getMessageToUpdate(); updateHeaderTime(&(imu_msg->header), descriptor_set, timestamp); - imu_msg->linear_acceleration.x = USTRAIN_G * delta_velocity.delta_velocity[0] / delta_time; - imu_msg->linear_acceleration.y = USTRAIN_G * delta_velocity.delta_velocity[1] / delta_time; - imu_msg->linear_acceleration.z = USTRAIN_G * delta_velocity.delta_velocity[2] / delta_time; - if (config_->use_enu_frame_) - { - imu_msg->linear_acceleration.y *= -1.0; - imu_msg->linear_acceleration.z *= -1.0; - } + const tf2::Vector3 linear_acceleration_in_microstrain_vehicle_frame( + USTRAIN_G * delta_velocity.delta_velocity[0] / delta_time, + USTRAIN_G * delta_velocity.delta_velocity[1] / delta_time, + USTRAIN_G * delta_velocity.delta_velocity[2] / delta_time + ); + const tf2::Vector3& linear_acceleration_in_driver_vehicle_frame = config_->microstrain_vehicle_frame_to_driver_vehicle_frame_transform_tf_ * linear_acceleration_in_microstrain_vehicle_frame; + imu_msg->linear_acceleration.x = linear_acceleration_in_driver_vehicle_frame.getX(); + imu_msg->linear_acceleration.y = linear_acceleration_in_driver_vehicle_frame.getY(); + imu_msg->linear_acceleration.z = linear_acceleration_in_driver_vehicle_frame.getZ(); } void Publishers::handleSensorCompQuaternion(const mip::data_sensor::CompQuaternion& comp_quaternion, const uint8_t descriptor_set, mip::Timestamp timestamp) @@ -677,30 +662,20 @@ void Publishers::handleSensorCompQuaternion(const mip::data_sensor::CompQuaterni // Rotate the quaternion into the correct frame auto imu_msg = imu_pub_->getMessageToUpdate(); updateHeaderTime(&(imu_msg->header), descriptor_set, timestamp); - const tf2::Transform ned_to_microstrain_vehicle_transform_tf(tf2::Quaternion(comp_quaternion.q[1], comp_quaternion.q[2], comp_quaternion.q[3], comp_quaternion.q[0])); - if (config_->use_enu_frame_) - { - const tf2::Transform ros_vehicle_to_enu_transform_tf = config_->ned_to_enu_transform_tf_ * ned_to_microstrain_vehicle_transform_tf.inverse() * config_->ros_vehicle_to_microstrain_vehicle_transform_tf_; - imu_msg->orientation = tf2::toMsg(ros_vehicle_to_enu_transform_tf.getRotation()); - } - else - { - imu_msg->orientation = tf2::toMsg(ned_to_microstrain_vehicle_transform_tf.getRotation().inverse()); - } + const tf2::Transform microstrain_vehicle_frame_to_microstrain_global_frame_transform_tf(tf2::Quaternion(comp_quaternion.q[1], comp_quaternion.q[2], comp_quaternion.q[3], comp_quaternion.q[0])); + const tf2::Transform& driver_vehicle_frame_to_driver_global_frame_transform_tf = config_->microstrain_global_frame_to_driver_global_frame_transform_tf_ * microstrain_vehicle_frame_to_microstrain_global_frame_transform_tf * config_->driver_vehicle_frame_to_microstrain_vehicle_frame_transform_tf_; + imu_msg->orientation = tf2::toMsg(driver_vehicle_frame_to_driver_global_frame_transform_tf.getRotation()); } void Publishers::handleSensorScaledMag(const mip::data_sensor::ScaledMag& scaled_mag, const uint8_t descriptor_set, mip::Timestamp timestamp) { auto mag_msg = mag_pub_->getMessageToUpdate(); updateHeaderTime(&(mag_msg->header), descriptor_set, timestamp); - mag_msg->magnetic_field.x = scaled_mag.scaled_mag[0]; - mag_msg->magnetic_field.y = scaled_mag.scaled_mag[1]; - mag_msg->magnetic_field.z = scaled_mag.scaled_mag[2]; - if (config_->use_enu_frame_) - { - mag_msg->magnetic_field.y *= -1.0; - mag_msg->magnetic_field.z *= -1.0; - } + const tf2::Vector3 magnetic_field_in_microstrain_vehicle_frame(scaled_mag.scaled_mag[0], scaled_mag.scaled_mag[1], scaled_mag.scaled_mag[2]); + const tf2::Vector3& magnetic_field_in_driver_vehicle_frame = config_->microstrain_vehicle_frame_to_driver_vehicle_frame_transform_tf_ * magnetic_field_in_microstrain_vehicle_frame; + mag_msg->magnetic_field.x = magnetic_field_in_driver_vehicle_frame.getX(); + mag_msg->magnetic_field.y = magnetic_field_in_driver_vehicle_frame.getX(); + mag_msg->magnetic_field.z = magnetic_field_in_driver_vehicle_frame.getX(); } void Publishers::handleSensorScaledPressure(const mip::data_sensor::ScaledPressure& scaled_pressure, const uint8_t descriptor_set, mip::Timestamp timestamp) @@ -798,18 +773,11 @@ void Publishers::handleGnssVelNed(const mip::data_gnss::VelNed& vel_ned, const u // GNSS velocity message auto gnss_velocity_msg = gnss_velocity_pub_[gnss_index]->getMessageToUpdate(); updateHeaderTime(&(gnss_velocity_msg->header), descriptor_set, timestamp); - if (config_->use_enu_frame_) - { - gnss_velocity_msg->twist.twist.linear.x = vel_ned.v[1]; - gnss_velocity_msg->twist.twist.linear.y = vel_ned.v[0]; - gnss_velocity_msg->twist.twist.linear.z = -vel_ned.v[2]; - } - else - { - gnss_velocity_msg->twist.twist.linear.x = vel_ned.v[0]; - gnss_velocity_msg->twist.twist.linear.y = vel_ned.v[1]; - gnss_velocity_msg->twist.twist.linear.z = vel_ned.v[2]; - } + const tf2::Vector3 velocity_in_microstrain_global_frame(vel_ned.v[0], vel_ned.v[1], vel_ned.v[2]); + const tf2::Vector3& velocity_in_driver_global_frame = config_->microstrain_global_frame_to_driver_global_frame_transform_tf_ * velocity_in_microstrain_global_frame; + gnss_velocity_msg->twist.twist.linear.x = velocity_in_driver_global_frame.getX(); + gnss_velocity_msg->twist.twist.linear.y = velocity_in_driver_global_frame.getY(); + gnss_velocity_msg->twist.twist.linear.z = velocity_in_driver_global_frame.getZ(); gnss_velocity_msg->twist.covariance[0] = vel_ned.speed_accuracy; gnss_velocity_msg->twist.covariance[7] = vel_ned.speed_accuracy; gnss_velocity_msg->twist.covariance[14] = vel_ned.speed_accuracy; @@ -822,39 +790,31 @@ void Publishers::handleGnssVelNed(const mip::data_gnss::VelNed& vel_ned, const u config_->geocentric_converter_.Reverse(gnss_odometry_msg->pose.pose.position.x, gnss_odometry_msg->pose.pose.position.y, gnss_odometry_msg->pose.pose.position.z, lat, lon, alt); // Convert the course over ground to the correct frame - tf2::Quaternion ned_to_microstrain_vehicle_quaternion_tf; - ned_to_microstrain_vehicle_quaternion_tf.setRPY(0, 0, vel_ned.heading * M_PI / 180); - const tf2::Transform ned_to_microstrain_vehicle_transform_tf(ned_to_microstrain_vehicle_quaternion_tf); + tf2::Quaternion microstrain_global_frame_to_microstrain_vehicle_frame_quaternion_tf; + microstrain_global_frame_to_microstrain_vehicle_frame_quaternion_tf.setRPY(0, 0, vel_ned.heading * M_PI / 180); + const tf2::Transform microstrain_global_frame_to_microstrain_vehicle_frame_transform_tf(microstrain_global_frame_to_microstrain_vehicle_frame_quaternion_tf); + const tf2::Transform microstrain_vehicle_frame_to_microstrain_global_frame_transform_tf = microstrain_global_frame_to_microstrain_vehicle_frame_transform_tf.inverse(); + + tf2::Transform earth_to_driver_global_frame_transform_tf; if (config_->use_enu_frame_) - { - const tf2::Transform earth_to_enu_transform_tf(ecefToEnuTransform(lat, lon)); - const tf2::Transform ros_vehicle_to_earth_transform_tf = earth_to_enu_transform_tf.inverse() * config_->ned_to_enu_transform_tf_ * ned_to_microstrain_vehicle_transform_tf.inverse() * config_->ros_vehicle_to_microstrain_vehicle_transform_tf_; - gnss_odometry_msg->pose.pose.orientation = tf2::toMsg(ros_vehicle_to_earth_transform_tf.getRotation()); - } + earth_to_driver_global_frame_transform_tf = tf2::Transform(ecefToEnuTransform(lat, lon)); else - { - const tf2::Transform earth_to_ned_transform_tf(ecefToNedTransform(lat, lon)); - const tf2::Transform microstrain_vehicle_to_earth_transform_tf = earth_to_ned_transform_tf.inverse() * ned_to_microstrain_vehicle_transform_tf.inverse(); - gnss_odometry_msg->pose.pose.orientation = tf2::toMsg(microstrain_vehicle_to_earth_transform_tf.getRotation()); - } + earth_to_driver_global_frame_transform_tf = tf2::Transform(ecefToNedTransform(lat, lon)); + const tf2::Transform& driver_global_frame_to_earth_transform_tf = earth_to_driver_global_frame_transform_tf.inverse(); + const tf2::Transform& driver_vehicle_frame_to_earth_transform_tf = + driver_global_frame_to_earth_transform_tf * + config_->microstrain_global_frame_to_driver_global_frame_transform_tf_ * + microstrain_vehicle_frame_to_microstrain_global_frame_transform_tf * + config_->driver_vehicle_frame_to_microstrain_vehicle_frame_transform_tf_; + gnss_odometry_msg->pose.pose.orientation = tf2::toMsg(driver_vehicle_frame_to_earth_transform_tf.getRotation()); + gnss_odometry_msg->pose.covariance[35] = vel_ned.heading_accuracy; // Rotate the velocity to the sensor frame for the odometry message - const tf2::Vector3 imu_velocity_in_ned_frame(vel_ned.v[0], vel_ned.v[1], vel_ned.v[2]); - if (config_->use_enu_frame_) - { - const tf2::Vector3 imu_velocity_in_ros_vehicle_frame = config_->ros_vehicle_to_microstrain_vehicle_transform_tf_.inverse() * ned_to_microstrain_vehicle_transform_tf * imu_velocity_in_ned_frame; - gnss_odometry_msg->twist.twist.linear.x = imu_velocity_in_ros_vehicle_frame.getX(); - gnss_odometry_msg->twist.twist.linear.y = imu_velocity_in_ros_vehicle_frame.getY(); - gnss_odometry_msg->twist.twist.linear.z = imu_velocity_in_ros_vehicle_frame.getZ(); - } - else - { - const tf2::Vector3 imu_velocity_in_microstrain_vehicle_frame = ned_to_microstrain_vehicle_transform_tf * imu_velocity_in_ned_frame; - gnss_odometry_msg->twist.twist.linear.x = imu_velocity_in_microstrain_vehicle_frame.getX(); - gnss_odometry_msg->twist.twist.linear.y = imu_velocity_in_microstrain_vehicle_frame.getY(); - gnss_odometry_msg->twist.twist.linear.z = imu_velocity_in_microstrain_vehicle_frame.getZ(); - } + const tf2::Vector3 velocity_in_driver_vehicle_frame = config_->microstrain_vehicle_frame_to_driver_vehicle_frame_transform_tf_ * microstrain_global_frame_to_microstrain_vehicle_frame_transform_tf * velocity_in_microstrain_global_frame; + gnss_odometry_msg->twist.twist.linear.x = velocity_in_driver_vehicle_frame.getX(); + gnss_odometry_msg->twist.twist.linear.y = velocity_in_driver_vehicle_frame.getY(); + gnss_odometry_msg->twist.twist.linear.z = velocity_in_driver_vehicle_frame.getZ(); gnss_odometry_msg->twist.covariance = gnss_velocity_msg->twist.covariance; } @@ -1333,7 +1293,7 @@ void Publishers::handleFilterPositionLlhUncertainty(const mip::data_filter::Posi const double n = pow(position_llh_uncertainty.north, 2); const double e = pow(position_llh_uncertainty.east, 2); const double d = pow(position_llh_uncertainty.down, 2); - const PoseWithCovarianceStampedMsg::_pose_type::_covariance_type ned_frame_covariance = + const PoseWithCovarianceStampedMsg::_pose_type::_covariance_type microstrain_global_frame_covariance = { n, 0, 0, 0, 0, 0, 0, e, 0, 0, 0, 0, @@ -1342,37 +1302,25 @@ void Publishers::handleFilterPositionLlhUncertainty(const mip::data_filter::Posi 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0 }; - const PoseWithCovarianceStampedMsg::_pose_type::_covariance_type enu_frame_covariance = tf2::transformCovariance(ned_frame_covariance, config_->ned_to_enu_transform_tf_); + const PoseWithCovarianceStampedMsg::_pose_type::_covariance_type driver_global_frame_covariance = tf2::transformCovariance(microstrain_global_frame_covariance, config_->microstrain_global_frame_to_driver_global_frame_transform_tf_); // Filter fix message auto filter_llh_position_msg = filter_llh_position_pub_->getMessageToUpdate(); updateHeaderTime(&(filter_llh_position_msg->header), descriptor_set, timestamp); filter_llh_position_msg->position_covariance_type = NavSatFixMsg::COVARIANCE_TYPE_DIAGONAL_KNOWN; - if (config_->use_enu_frame_) - { - filter_llh_position_msg->position_covariance[0] = enu_frame_covariance[0]; - filter_llh_position_msg->position_covariance[4] = enu_frame_covariance[7]; - filter_llh_position_msg->position_covariance[8] = enu_frame_covariance[14]; - } - else - { - filter_llh_position_msg->position_covariance[0] = ned_frame_covariance[0]; - filter_llh_position_msg->position_covariance[4] = ned_frame_covariance[7]; - filter_llh_position_msg->position_covariance[8] = ned_frame_covariance[14]; - } + filter_llh_position_msg->position_covariance[0] = driver_global_frame_covariance[0]; + filter_llh_position_msg->position_covariance[4] = driver_global_frame_covariance[7]; + filter_llh_position_msg->position_covariance[8] = driver_global_frame_covariance[14]; // Filter relative odometry message (not counted as updating) auto filter_odometry_map_msg = filter_odometry_map_pub_->getMessage(); - if (config_->use_enu_frame_) - setTranslationCovarianceOnCovariance(&filter_odometry_map_msg->pose.covariance, getTranslationCovarianceFromCovariance(enu_frame_covariance)); - else - setTranslationCovarianceOnCovariance(&filter_odometry_map_msg->pose.covariance, getTranslationCovarianceFromCovariance(ned_frame_covariance)); + setTranslationCovarianceOnCovariance(&filter_odometry_map_msg->pose.covariance, getTranslationCovarianceFromCovariance(driver_global_frame_covariance)); // If the device does not support ECEF uncertainty, rotate this uncertainty into the ECEF frame and process it if (!supports_filter_ecef_) { - const tf2::Transform ecef_to_ned_transform(ecefToNedTransform(filter_llh_position_msg->latitude, filter_llh_position_msg->longitude)); - const PoseWithCovarianceStampedMsg::_pose_type::_covariance_type ecef_frame_covariance = tf2::transformCovariance(ned_frame_covariance, ecef_to_ned_transform.inverse()); + const tf2::Transform microstrain_global_frame_to_ecef_frame_transform_tf(ecefToNedTransform(filter_llh_position_msg->latitude, filter_llh_position_msg->longitude).inverse()); + const PoseWithCovarianceStampedMsg::_pose_type::_covariance_type ecef_frame_covariance = tf2::transformCovariance(microstrain_global_frame_covariance, microstrain_global_frame_to_ecef_frame_transform_tf); mip::data_filter::EcefPosUncertainty ecef_pos_uncertainty; ecef_pos_uncertainty.pos_uncertainty = { @@ -1386,6 +1334,23 @@ void Publishers::handleFilterPositionLlhUncertainty(const mip::data_filter::Posi void Publishers::handleFilterAttitudeQuaternion(const mip::data_filter::AttitudeQuaternion& attitude_quaternion, const uint8_t descriptor_set, mip::Timestamp timestamp) { + // Filtered IMU message + auto filter_imu_msg = filter_imu_pub_->getMessageToUpdate(); + updateHeaderTime(&(filter_imu_msg->header), descriptor_set, timestamp); + + // Filter odometry map message (not counted as updating) + auto filter_odometry_map_msg = filter_odometry_map_pub_->getMessage(); + + // Put the orientation into the NED/ENU frame for our map messages + const tf2::Transform microstrain_vehicle_frame_to_microstrain_global_frame_transform_tf(tf2::Quaternion(attitude_quaternion.q[1], attitude_quaternion.q[2], attitude_quaternion.q[3], attitude_quaternion.q[0])); + const tf2::Transform& driver_vehicle_frame_to_driver_global_frame_transform_tf = config_->microstrain_global_frame_to_driver_global_frame_transform_tf_ * microstrain_vehicle_frame_to_microstrain_global_frame_transform_tf * config_->driver_vehicle_frame_to_microstrain_vehicle_frame_transform_tf_; + filter_odometry_map_msg->pose.pose.orientation = tf2::toMsg(driver_vehicle_frame_to_driver_global_frame_transform_tf.getRotation()); + filter_imu_msg->orientation = filter_odometry_map_msg->pose.pose.orientation; + + imu_link_to_map_transform_tf_stamped_.setBasis(driver_vehicle_frame_to_driver_global_frame_transform_tf.getBasis()); + imu_link_to_map_transform_tf_stamped_.stamp_ = tf2_ros::fromMsg(rosTimeNow(node_)); + imu_link_to_map_transform_attitude_updated_ = true; + // Filter odometry message rotated to ECEF (not counted as updating) auto filter_odometry_earth_msg = filter_odometry_earth_pub_->getMessage(); @@ -1394,47 +1359,14 @@ void Publishers::handleFilterAttitudeQuaternion(const mip::data_filter::Attitude config_->geocentric_converter_.Reverse(filter_odometry_earth_msg->pose.pose.position.x, filter_odometry_earth_msg->pose.pose.position.y, filter_odometry_earth_msg->pose.pose.position.z, lat, lon, alt); // Put the orientation into the ECEF frame for our earth messages - const tf2::Transform microstrain_vehicle_to_ned_transform_tf(tf2::Quaternion(attitude_quaternion.q[1], attitude_quaternion.q[2], attitude_quaternion.q[3], attitude_quaternion.q[0])); - if (config_->use_enu_frame_) - { - const tf2::Transform earth_to_enu_transform_tf(ecefToEnuTransform(lat, lon)); - const tf2::Transform ros_vehicle_to_earth_transform_tf = earth_to_enu_transform_tf.inverse() * config_->ned_to_enu_transform_tf_ * microstrain_vehicle_to_ned_transform_tf * config_->ros_vehicle_to_microstrain_vehicle_transform_tf_; - imu_link_to_earth_transform_tf_stamped_.setBasis(ros_vehicle_to_earth_transform_tf.getBasis()); - filter_odometry_earth_msg->pose.pose.orientation = tf2::toMsg(ros_vehicle_to_earth_transform_tf.getRotation()); - } - else - { - const tf2::Transform earth_to_ned_transform_tf(ecefToNedTransform(lat, lon)); - const tf2::Transform microstrain_vehicle_to_earth_transform_tf = earth_to_ned_transform_tf.inverse() * microstrain_vehicle_to_ned_transform_tf; - imu_link_to_earth_transform_tf_stamped_.setBasis(microstrain_vehicle_to_earth_transform_tf.getBasis()); - filter_odometry_earth_msg->pose.pose.orientation = tf2::toMsg(microstrain_vehicle_to_earth_transform_tf.getRotation()); - } + const tf2::Transform& driver_global_frame_to_earth_transform_tf = config_->use_enu_frame_ ? tf2::Transform(ecefToEnuTransform(lat, lon).inverse()) : tf2::Transform(ecefToNedTransform(lat, lon).inverse()); + const tf2::Transform& driver_vehicle_frame_to_earth_frame_transform_tf = driver_global_frame_to_earth_transform_tf * driver_vehicle_frame_to_driver_global_frame_transform_tf; + filter_odometry_earth_msg->pose.pose.orientation = tf2::toMsg(driver_vehicle_frame_to_earth_frame_transform_tf.getRotation()); + + imu_link_to_earth_transform_tf_stamped_.setBasis(driver_vehicle_frame_to_earth_frame_transform_tf.getBasis()); imu_link_to_earth_transform_tf_stamped_.stamp_ = tf2_ros::fromMsg(rosTimeNow(node_)); imu_link_to_earth_transform_attitude_updated_ = true; - // Filtered IMU message - auto filter_imu_msg = filter_imu_pub_->getMessageToUpdate(); - updateHeaderTime(&(filter_imu_msg->header), descriptor_set, timestamp); - - // Filter odometry map message (not counted as updating) - auto filter_odometry_map_msg = filter_odometry_map_pub_->getMessage(); - - // Put the orientation into the NED/ENU frame for our map messages - if (config_->use_enu_frame_) - { - const tf2::Transform ros_vehicle_to_enu_transform_tf = config_->ned_to_enu_transform_tf_ * microstrain_vehicle_to_ned_transform_tf * config_->ros_vehicle_to_microstrain_vehicle_transform_tf_; - imu_link_to_map_transform_tf_stamped_.setBasis(ros_vehicle_to_enu_transform_tf.getBasis()); - filter_odometry_map_msg->pose.pose.orientation = tf2::toMsg(ros_vehicle_to_enu_transform_tf.getRotation()); - filter_imu_msg->orientation = tf2::toMsg(ros_vehicle_to_enu_transform_tf.getRotation()); - } - else - { - imu_link_to_map_transform_tf_stamped_.setBasis(microstrain_vehicle_to_ned_transform_tf.getBasis()); - filter_odometry_map_msg->pose.pose.orientation = tf2::toMsg(microstrain_vehicle_to_ned_transform_tf.getRotation()); - filter_imu_msg->orientation = tf2::toMsg(microstrain_vehicle_to_ned_transform_tf.getRotation()); - } - imu_link_to_map_transform_tf_stamped_.stamp_ = tf2_ros::fromMsg(rosTimeNow(node_)); - imu_link_to_map_transform_attitude_updated_ = true; } void Publishers::handleFilterEulerAnglesUncertainty(const mip::data_filter::EulerAnglesUncertainty& euler_angles_uncertainty, const uint8_t descriptor_set, mip::Timestamp timestamp) @@ -1445,7 +1377,7 @@ void Publishers::handleFilterEulerAnglesUncertainty(const mip::data_filter::Eule const double r = pow(euler_angles_uncertainty.roll, 2); const double p = pow(euler_angles_uncertainty.pitch, 2); const double y = pow(euler_angles_uncertainty.yaw, 2); - const PoseWithCovarianceStampedMsg::_pose_type::_covariance_type ned_frame_covariance = + const PoseWithCovarianceStampedMsg::_pose_type::_covariance_type microstrain_global_frame_covariance = { 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, @@ -1454,15 +1386,12 @@ void Publishers::handleFilterEulerAnglesUncertainty(const mip::data_filter::Eule 0, 0, 0, 0, p, 0, 0, 0, 0, 0, 0, y }; - const PoseWithCovarianceStampedMsg::_pose_type::_covariance_type enu_frame_covariance = tf2::transformCovariance(ned_frame_covariance, config_->ned_to_enu_transform_tf_); + const PoseWithCovarianceStampedMsg::_pose_type::_covariance_type driver_global_frame_covariance = tf2::transformCovariance(microstrain_global_frame_covariance, config_->microstrain_global_frame_to_driver_global_frame_transform_tf_); // Filtered IMU message auto filter_imu_msg = filter_imu_pub_->getMessageToUpdate(); updateHeaderTime(&(filter_imu_msg->header), descriptor_set, timestamp); - if (config_->use_enu_frame_) - filter_imu_msg->orientation_covariance = getRotationCovarianceFromCovariance(enu_frame_covariance); - else - filter_imu_msg->orientation_covariance = getRotationCovarianceFromCovariance(ned_frame_covariance); + filter_imu_msg->orientation_covariance = getRotationCovarianceFromCovariance(driver_global_frame_covariance); // Filter odometry message (not counted as updating) auto filter_odometry_earth_msg = filter_odometry_earth_pub_->getMessage(); @@ -1471,16 +1400,13 @@ void Publishers::handleFilterEulerAnglesUncertainty(const mip::data_filter::Eule // NOTE: We view the NED frame and microstrain vehicle frame as the same here since there is no transform between them. double lat, lon, height; config_->geocentric_converter_.Reverse(filter_odometry_earth_msg->pose.pose.position.x, filter_odometry_earth_msg->pose.pose.position.y, filter_odometry_earth_msg->pose.pose.position.z, lat, lon, height); - const tf2::Transform ecef_to_ned_transform(ecefToNedTransform(lat, lon)); - const PoseWithCovarianceStampedMsg::_pose_type::_covariance_type ecef_frame_covariance = tf2::transformCovariance(ned_frame_covariance, ecef_to_ned_transform.inverse()); + const tf2::Transform microstrain_global_frame_to_ecef_frame_transform_tf(ecefToNedTransform(lat, lon).inverse()); + const PoseWithCovarianceStampedMsg::_pose_type::_covariance_type ecef_frame_covariance = tf2::transformCovariance(microstrain_global_frame_covariance, microstrain_global_frame_to_ecef_frame_transform_tf); setRotationCovarianceOnCovariance(&filter_odometry_earth_msg->pose.covariance, getRotationCovarianceFromCovariance(ecef_frame_covariance)); // Filter relative odometry message (not counted as updating) auto filter_odometry_map_msg = filter_odometry_map_pub_->getMessage(); - if (config_->use_enu_frame_) - setRotationCovarianceOnCovariance(&filter_odometry_map_msg->pose.covariance, getRotationCovarianceFromCovariance(enu_frame_covariance)); - else - setRotationCovarianceOnCovariance(&filter_odometry_map_msg->pose.covariance, getRotationCovarianceFromCovariance(ned_frame_covariance)); + setRotationCovarianceOnCovariance(&filter_odometry_map_msg->pose.covariance, getRotationCovarianceFromCovariance(driver_global_frame_covariance)); } void Publishers::handleFilterVelocityNed(const mip::data_filter::VelocityNed& velocity_ned, const uint8_t descriptor_set, mip::Timestamp timestamp) @@ -1721,44 +1647,18 @@ void Publishers::handleFilterMultiAntennaOffsetCorrection(const mip::data_filter mip_filter_multi_antenna_offset_correction_msg->offset[2] = multi_antenna_offset_correction.offset[2]; mip_filter_multi_antenna_offset_correction_pub_->publish(*mip_filter_multi_antenna_offset_correction_msg); - const tf2::Transform gnss_x_antenna_correction_to_microstrain_vehicle_tf(tf2::Quaternion::getIdentity(), tf2::Vector3(multi_antenna_offset_correction.offset[0], multi_antenna_offset_correction.offset[1], multi_antenna_offset_correction.offset[2])); + const tf2::Transform gnss_x_antenna_correction_to_microstrain_vehicle_frame_tf(tf2::Quaternion::getIdentity(), tf2::Vector3(multi_antenna_offset_correction.offset[0], multi_antenna_offset_correction.offset[1], multi_antenna_offset_correction.offset[2])); + const tf2::Transform gnss_x_antenna_correction_to_driver_vehicle_frame_tf = config_->microstrain_vehicle_frame_to_driver_vehicle_frame_transform_tf_ * gnss_x_antenna_correction_to_microstrain_vehicle_frame_tf; TransformStampedMsg gnss_x_antenna_to_imu_link_transform = gnss_antenna_link_to_imu_link_transform_[multi_antenna_offset_correction.receiver_id - 1]; gnss_x_antenna_to_imu_link_transform.header.stamp = rosTimeNow(node_); - if (config_->use_enu_frame_) - { - const tf2::Transform gnss_x_antenna_correction_to_ros_vehicle_tf = config_->ros_vehicle_to_microstrain_vehicle_transform_tf_.inverse() * gnss_x_antenna_correction_to_microstrain_vehicle_tf; - gnss_x_antenna_to_imu_link_transform.transform.translation.x += gnss_x_antenna_correction_to_ros_vehicle_tf.getOrigin().getX(); - gnss_x_antenna_to_imu_link_transform.transform.translation.y += gnss_x_antenna_correction_to_ros_vehicle_tf.getOrigin().getY(); - gnss_x_antenna_to_imu_link_transform.transform.translation.z += gnss_x_antenna_correction_to_ros_vehicle_tf.getOrigin().getZ(); - } - else - { - gnss_x_antenna_to_imu_link_transform.transform.translation.x += gnss_x_antenna_correction_to_microstrain_vehicle_tf.getOrigin().getX(); - gnss_x_antenna_to_imu_link_transform.transform.translation.y += gnss_x_antenna_correction_to_microstrain_vehicle_tf.getOrigin().getY(); - gnss_x_antenna_to_imu_link_transform.transform.translation.z += gnss_x_antenna_correction_to_microstrain_vehicle_tf.getOrigin().getZ(); - } - static_transform_broadcaster_->sendTransform(gnss_x_antenna_to_imu_link_transform); + gnss_x_antenna_to_imu_link_transform.transform.translation.x += gnss_x_antenna_correction_to_driver_vehicle_frame_tf.getOrigin().getX(); + gnss_x_antenna_to_imu_link_transform.transform.translation.y += gnss_x_antenna_correction_to_driver_vehicle_frame_tf.getOrigin().getY(); + gnss_x_antenna_to_imu_link_transform.transform.translation.z += gnss_x_antenna_correction_to_driver_vehicle_frame_tf.getOrigin().getZ(); + transform_broadcaster_->sendTransform(gnss_x_antenna_to_imu_link_transform); } void Publishers::handleFilterGnssDualAntennaStatus(const mip::data_filter::GnssDualAntennaStatus& gnss_dual_antenna_status, const uint8_t descriptor_set, mip::Timestamp timestamp) { - // Filter Dual Antenna Status (pose version) - auto filter_dual_antenna_heading_msg = filter_dual_antenna_heading_pub_->getMessageToUpdate(); - updateHeaderTime(&(filter_dual_antenna_heading_msg->header), descriptor_set, timestamp); - tf2::Quaternion microstrain_vehicle_to_ned_quaternion_tf; - microstrain_vehicle_to_ned_quaternion_tf.setRPY(0, 0, gnss_dual_antenna_status.heading); - tf2::Transform microstrain_vehicle_to_ned_transform_tf(microstrain_vehicle_to_ned_quaternion_tf); - if (config_->use_enu_frame_) - { - const tf2::Transform ros_vehicle_to_enu_transform_tf = config_->ned_to_enu_transform_tf_ * microstrain_vehicle_to_ned_transform_tf * config_->ros_vehicle_to_microstrain_vehicle_transform_tf_; - filter_dual_antenna_heading_msg->pose.pose.orientation = tf2::toMsg(ros_vehicle_to_enu_transform_tf.getRotation()); - } - else - { - filter_dual_antenna_heading_msg->pose.pose.orientation = tf2::toMsg(microstrain_vehicle_to_ned_transform_tf.getRotation()); - } - filter_dual_antenna_heading_msg->pose.covariance[35] = gnss_dual_antenna_status.heading_unc; - // Filter GNSS Dual Antenna status auto mip_filter_gnss_dual_antenna_status_msg = mip_filter_gnss_dual_antenna_status_pub_->getMessage(); updateMipHeader(&(mip_filter_gnss_dual_antenna_status_msg->header), descriptor_set); @@ -1772,6 +1672,16 @@ void Publishers::handleFilterGnssDualAntennaStatus(const mip::data_filter::GnssD mip_filter_gnss_dual_antenna_status_msg->valid_flags = gnss_dual_antenna_status.valid_flags; mip_filter_gnss_dual_antenna_status_pub_->publish(*mip_filter_gnss_dual_antenna_status_msg); + // Filter Dual Antenna Status (pose version) + auto filter_dual_antenna_heading_msg = filter_dual_antenna_heading_pub_->getMessageToUpdate(); + updateHeaderTime(&(filter_dual_antenna_heading_msg->header), descriptor_set, timestamp); + tf2::Quaternion microstrain_vehicle_frame_to_microstrain_global_frame_quaternion_tf; + microstrain_vehicle_frame_to_microstrain_global_frame_quaternion_tf.setRPY(0, 0, gnss_dual_antenna_status.heading); + const tf2::Transform microstrain_vehicle_frame_to_microstrain_global_frame_transform_tf(microstrain_vehicle_frame_to_microstrain_global_frame_quaternion_tf); + const tf2::Transform driver_vehicle_frame_to_driver_global_frame_transform_tf = config_->microstrain_global_frame_to_driver_global_frame_transform_tf_ * microstrain_vehicle_frame_to_microstrain_global_frame_transform_tf * config_->driver_vehicle_frame_to_microstrain_vehicle_frame_transform_tf_; + filter_dual_antenna_heading_msg->pose.pose.orientation = tf2::toMsg(driver_vehicle_frame_to_driver_global_frame_transform_tf.getRotation()); + filter_dual_antenna_heading_msg->pose.covariance[35] = gnss_dual_antenna_status.heading_unc; + // Filter Human Readable status auto filter_human_readable_status_msg = filter_human_readable_status_pub_->getMessage(); if (gnss_dual_antenna_status.fix_type == mip::data_filter::GnssDualAntennaStatus::FixType::FIX_NONE) diff --git a/src/subscribers.cpp b/src/subscribers.cpp index 8b41027..b7f9f67 100644 --- a/src/subscribers.cpp +++ b/src/subscribers.cpp @@ -349,7 +349,7 @@ void Subscribers::externalHeadingEnuCallback(const PoseWithCovarianceStampedMsg& double r, p, y; tf2::Quaternion q; tf2::fromMsg(heading.pose.pose.orientation, q); - q = config_->ned_to_enu_transform_tf_.inverse() * q; + //q = config_->ned_to_enu_transform_tf_.inverse() * q; // TODO: Fix this tf2::Matrix3x3 m(q); m.getRPY(r, p, y); @@ -461,47 +461,36 @@ uint8_t Subscribers::getSensorIdFromFrameId(const std::string& frame_id) RosTimeType frame_time; setRosTime(&frame_time, 0, 0); if (transform_buffer_->canTransform(config_->frame_id_, frame_id, frame_time, RosDurationType(0, 0), &tf_error_string)) { - // Populate the reference frame command from the transform. Convert from ROS vehicle frame to Microstrain vehicle frame if operating in enu mode - auto vehicle_to_sensor_transform = transform_buffer_->lookupTransform(config_->frame_id_, frame_id, frame_time); - if (config_->use_enu_frame_) - { - tf2::Transform ros_vehicle_to_sensor_transform_tf, microstrain_vehicle_to_sensor_transform_tf; - tf2::fromMsg(vehicle_to_sensor_transform.transform, ros_vehicle_to_sensor_transform_tf); - const tf2::Vector3& ros_vehicle_to_sensor_translation_tf = ros_vehicle_to_sensor_transform_tf.getOrigin(); - const tf2::Quaternion& ros_vehicle_to_sensor_rotation_tf = ros_vehicle_to_sensor_transform_tf.getRotation(); - const tf2::Vector3& microstrain_vehicle_to_sensor_translation_tf = config_->ros_vehicle_to_microstrain_vehicle_transform_tf_ * ros_vehicle_to_sensor_translation_tf; - const tf2::Quaternion& microstrain_vehicle_to_sensor_rotation_tf = config_->ros_vehicle_to_microstrain_vehicle_transform_tf_ * ros_vehicle_to_sensor_rotation_tf; - microstrain_vehicle_to_sensor_transform_tf.setOrigin(microstrain_vehicle_to_sensor_translation_tf); - microstrain_vehicle_to_sensor_transform_tf.setRotation(microstrain_vehicle_to_sensor_rotation_tf); - vehicle_to_sensor_transform.transform = tf2::toMsg(microstrain_vehicle_to_sensor_transform_tf); - } + // Convert the transform from the driver vehicle frame to the microstrain vehicle frame + const auto& driver_vehicle_frame_to_sensor_transform = transform_buffer_->lookupTransform(config_->frame_id_, frame_id, frame_time); + const tf2::Vector3 driver_vehicle_frame_to_sensor_translation_tf(driver_vehicle_frame_to_sensor_transform.transform.translation.x, driver_vehicle_frame_to_sensor_transform.transform.translation.y, driver_vehicle_frame_to_sensor_transform.transform.translation.z); + const tf2::Quaternion driver_vehicle_frame_to_sensor_rotation_tf(driver_vehicle_frame_to_sensor_transform.transform.rotation.x, driver_vehicle_frame_to_sensor_transform.transform.rotation.y, driver_vehicle_frame_to_sensor_transform.transform.rotation.z, driver_vehicle_frame_to_sensor_transform.transform.rotation.w); + const tf2::Vector3& microstrain_vehicle_frame_to_sensor_translation_tf = config_->driver_vehicle_frame_to_microstrain_vehicle_frame_transform_tf_ * driver_vehicle_frame_to_sensor_translation_tf; + const tf2::Quaternion& microstrain_vehicle_frame_to_sensor_rotation_tf = config_->driver_vehicle_frame_to_microstrain_vehicle_frame_transform_tf_ * driver_vehicle_frame_to_sensor_rotation_tf; + + // Populate the frame config command with the transform in the microstrain vehicle frame mip::commands_aiding::FrameConfig frame_config; frame_config.format = mip::commands_aiding::FrameConfig::Format::QUATERNION; frame_config.frame_id = external_frame_ids_size_ + 1; frame_config.function = mip::FunctionSelector::WRITE; frame_config.translation = { - static_cast(vehicle_to_sensor_transform.transform.translation.x), - static_cast(vehicle_to_sensor_transform.transform.translation.y), - static_cast(vehicle_to_sensor_transform.transform.translation.z) + static_cast(microstrain_vehicle_frame_to_sensor_translation_tf.getX()), + static_cast(microstrain_vehicle_frame_to_sensor_translation_tf.getY()), + static_cast(microstrain_vehicle_frame_to_sensor_translation_tf.getZ()) }; frame_config.rotation.quaternion = { - static_cast(vehicle_to_sensor_transform.transform.rotation.w), - static_cast(vehicle_to_sensor_transform.transform.rotation.x), - static_cast(vehicle_to_sensor_transform.transform.rotation.y), - static_cast(vehicle_to_sensor_transform.transform.rotation.z) + static_cast(microstrain_vehicle_frame_to_sensor_rotation_tf.getW()), + static_cast(microstrain_vehicle_frame_to_sensor_rotation_tf.getX()), + static_cast(microstrain_vehicle_frame_to_sensor_rotation_tf.getY()), + static_cast(microstrain_vehicle_frame_to_sensor_rotation_tf.getZ()) }; // This is a little expensive to do for debug, but should only happen rarely - const tf2::Quaternion q( - vehicle_to_sensor_transform.transform.rotation.x, - vehicle_to_sensor_transform.transform.rotation.y, - vehicle_to_sensor_transform.transform.rotation.z, - vehicle_to_sensor_transform.transform.rotation.w); - const tf2::Matrix3x3 m(q); + const tf2::Matrix3x3 m(microstrain_vehicle_frame_to_sensor_rotation_tf); double r, p, y; m.getRPY(r, p, y); MICROSTRAIN_INFO(node_, "Associating MIP frame %u with ROS frame %s", frame_config.frame_id, frame_id.c_str()); - MICROSTRAIN_INFO(node_, " Front (meters): %f, Right (meters): %f, Down (meters): %f", vehicle_to_sensor_transform.transform.translation.x, vehicle_to_sensor_transform.transform.translation.y, vehicle_to_sensor_transform.transform.translation.z); + MICROSTRAIN_INFO(node_, " Front (meters): %f, Right (meters): %f, Down (meters): %f", frame_config.translation[0], frame_config.translation[1], frame_config.translation[2]); MICROSTRAIN_INFO(node_, " Roll (degrees): %f, Pitch (degrees): %f, Yaw (degrees): %f", r * 180 / M_PI, p * 180 / M_PI, y * 180 / M_PI); // Attempt to send the command From 04c7992d3c8e961de2685375ac75306283cd308f Mon Sep 17 00:00:00 2001 From: robbiefish Date: Wed, 14 Aug 2024 19:21:23 +0000 Subject: [PATCH 2/2] Fixes external heading callback --- .../config.h | 11 ++++-- src/config.cpp | 35 +++++++++---------- src/subscribers.cpp | 2 +- 3 files changed, 26 insertions(+), 22 deletions(-) diff --git a/include/microstrain_inertial_driver_common/config.h b/include/microstrain_inertial_driver_common/config.h index 3769d2a..e6af6dc 100644 --- a/include/microstrain_inertial_driver_common/config.h +++ b/include/microstrain_inertial_driver_common/config.h @@ -95,6 +95,14 @@ class Config // Whether we will use the ROS vehicle or Microstrain vehicle frame for local frame data bool use_ros_vehicle_frame_; + // Transform used to transform from the NED frame to the ENU frame + tf2::Transform ned_to_enu_transform_tf_; + tf2::Transform enu_to_ned_transform_tf_; // This will just be the inverse of the above transform + + // Transform used to transform from the MicroStrain vehicle frame to the ROS vehicle frame + tf2::Transform microstrain_vehicle_frame_to_ros_vehicle_frame_transform_tf_; + tf2::Transform ros_vehicle_frame_to_microstrain_vehicle_frame_transform_tf_; // This will just be the inverse of the above transform + // Transform used to transform from the microstrain global frame to the driver global frame. // If the driver is configured with use_enu_frame = true, this will transform NED -> ENU // If the driver is configured with use_enu_frame = false, this will transform NED -> NED (identity transform) @@ -107,9 +115,6 @@ class Config tf2::Transform microstrain_vehicle_frame_to_driver_vehicle_frame_transform_tf_; tf2::Transform driver_vehicle_frame_to_microstrain_vehicle_frame_transform_tf_; // This will just be the inverse of the above transform - //tf2::Transform ned_to_enu_transform_tf_; - //tf2::Transform ros_vehicle_to_microstrain_vehicle_transform_tf_; - // Whether to enable the hardware odometer through the GPIO pins bool enable_hardware_odometer_; diff --git a/src/config.cpp b/src/config.cpp index d6b7584..e569375 100644 --- a/src/config.cpp +++ b/src/config.cpp @@ -40,6 +40,21 @@ Config::Config(RosNodeType* node) : node_(node) // Initialize the transform buffer and listener ahead of time transform_buffer_ = createTransformBuffer(node_); transform_listener_ = createTransformListener(transform_buffer_); + + // Store some transform definitions + ned_to_enu_transform_tf_ = tf2::Transform(tf2::Matrix3x3( + 0, 1, 0, + 1, 0, 0, + 0, 0, -1 + )); + enu_to_ned_transform_tf_ = ned_to_enu_transform_tf_.inverse(); + + microstrain_vehicle_frame_to_ros_vehicle_frame_transform_tf_ = tf2::Transform(tf2::Matrix3x3( + 1, 0, 0, + 0, -1, 0, + 0, 0, -1 + )); + ros_vehicle_frame_to_microstrain_vehicle_frame_transform_tf_ = microstrain_vehicle_frame_to_ros_vehicle_frame_transform_tf_.inverse(); } bool Config::configure(RosNodeType* node) @@ -72,31 +87,15 @@ bool Config::configure(RosNodeType* node) // The definition of the "driver" world frame and vehicle frame differs depending on use_enu_frame and use_ros_vehicle_frame if (use_enu_frame_) - { - microstrain_global_frame_to_driver_global_frame_transform_tf_ = tf2::Transform(tf2::Matrix3x3( - 0, 1, 0, - 1, 0, 0, - 0, 0, -1 - )); - } + microstrain_global_frame_to_driver_global_frame_transform_tf_ = ned_to_enu_transform_tf_; else - { microstrain_global_frame_to_driver_global_frame_transform_tf_ = tf2::Transform::getIdentity(); - } driver_global_frame_to_microstrain_global_frame_transform_tf_ = microstrain_global_frame_to_driver_global_frame_transform_tf_.inverse(); if (use_ros_vehicle_frame_) - { - microstrain_vehicle_frame_to_driver_vehicle_frame_transform_tf_ = tf2::Transform(tf2::Matrix3x3( - 1, 0, 0, - 0, -1, 0, - 0, 0, -1 - )); - } + microstrain_vehicle_frame_to_driver_vehicle_frame_transform_tf_ = microstrain_vehicle_frame_to_ros_vehicle_frame_transform_tf_; else - { microstrain_vehicle_frame_to_driver_vehicle_frame_transform_tf_ = tf2::Transform::getIdentity(); - } driver_vehicle_frame_to_microstrain_vehicle_frame_transform_tf_ = microstrain_vehicle_frame_to_driver_vehicle_frame_transform_tf_.inverse(); // tf config diff --git a/src/subscribers.cpp b/src/subscribers.cpp index b7f9f67..a59d034 100644 --- a/src/subscribers.cpp +++ b/src/subscribers.cpp @@ -349,7 +349,7 @@ void Subscribers::externalHeadingEnuCallback(const PoseWithCovarianceStampedMsg& double r, p, y; tf2::Quaternion q; tf2::fromMsg(heading.pose.pose.orientation, q); - //q = config_->ned_to_enu_transform_tf_.inverse() * q; // TODO: Fix this + q = config_->enu_to_ned_transform_tf_ * q; tf2::Matrix3x3 m(q); m.getRPY(r, p, y);