diff --git a/include/microstrain_mips/microstrain_3dm.h b/include/microstrain_mips/microstrain_3dm.h index 94333c04..f5215b81 100644 --- a/include/microstrain_mips/microstrain_3dm.h +++ b/include/microstrain_mips/microstrain_3dm.h @@ -42,12 +42,12 @@ extern "C" #include #include - // ROS #include "ros/ros.h" #include "sensor_msgs/NavSatFix.h" #include "sensor_msgs/Imu.h" #include "geometry_msgs/PoseWithCovarianceStamped.h" +#include #include "geometry_msgs/Vector3.h" #include "nav_msgs/Odometry.h" #include "std_msgs/Int8.h" @@ -82,7 +82,6 @@ extern "C" #include "microstrain_mips/SetMagAdaptiveVals.h" #include "microstrain_mips/SetMagDipAdaptiveVals.h" - #define MIP_SDK_GX4_45_IMU_STANDARD_MODE 0x01 #define MIP_SDK_GX4_45_IMU_DIRECT_MODE 0x02 @@ -98,8 +97,6 @@ extern "C" #define GX5_25_DEVICE "3DM-GX5-25" #define GX5_15_DEVICE "3DM-GX5-15" - - /** * \brief Contains functions for micostrain driver */ @@ -240,134 +237,135 @@ namespace Microstrain private: - //! @brief Reset KF service callback - bool reset_callback(std_srvs::Empty::Request &req, - std_srvs::Empty::Response &resp); - //! @brief Convience for printing packet stats - void print_packet_stats(); - - - - // Variables/fields - //The primary device interface structure - mip_interface device_interface_; - base_device_info_field device_info; - u8 temp_string[20]; - - //Packet Counters (valid, timeout, and checksum errors) - u32 filter_valid_packet_count_; - u32 ahrs_valid_packet_count_; - u32 gps_valid_packet_count_; - - u32 filter_timeout_packet_count_; - u32 ahrs_timeout_packet_count_; - u32 gps_timeout_packet_count_; - - u32 filter_checksum_error_packet_count_; - u32 ahrs_checksum_error_packet_count_; - u32 gps_checksum_error_packet_count_; - - //Data field storage - //AHRS - mip_ahrs_scaled_gyro curr_ahrs_gyro_; - mip_ahrs_scaled_accel curr_ahrs_accel_; - mip_ahrs_scaled_mag curr_ahrs_mag_; - mip_ahrs_quaternion curr_ahrs_quaternion_; - //GPS - mip_gps_llh_pos curr_llh_pos_; - mip_gps_ned_vel curr_ned_vel_; - mip_gps_time curr_gps_time_; - - //FILTER - mip_filter_llh_pos curr_filter_pos_; - mip_filter_ned_velocity curr_filter_vel_; - mip_filter_attitude_euler_angles curr_filter_angles_; - mip_filter_attitude_quaternion curr_filter_quaternion_; - mip_filter_compensated_angular_rate curr_filter_angular_rate_; - mip_filter_llh_pos_uncertainty curr_filter_pos_uncertainty_; - mip_filter_ned_vel_uncertainty curr_filter_vel_uncertainty_; - mip_filter_euler_attitude_uncertainty curr_filter_att_uncertainty_; - mip_filter_status curr_filter_status_; - - // ROS - ros::Publisher gps_pub_; - ros::Publisher imu_pub_; - ros::Publisher nav_pub_; - ros::Publisher nav_status_pub_; - ros::Publisher bias_pub_; - ros::Publisher device_status_pub_; - sensor_msgs::NavSatFix gps_msg_; - sensor_msgs::Imu imu_msg_; - nav_msgs::Odometry nav_msg_; - std_msgs::Int16MultiArray nav_status_msg_; - geometry_msgs::Vector3 bias_msg_; - std::string gps_frame_id_; - std::string imu_frame_id_; - std::string odom_frame_id_; - std::string odom_child_frame_id_; - microstrain_mips::status_msg device_status_msg_; - bool publish_gps_; - bool publish_imu_; - bool publish_odom_; - bool publish_bias_; - std::vector imu_linear_cov_; - std::vector imu_angular_cov_; - std::vector imu_orientation_cov_; - - //Device Flags - bool GX5_15; - bool GX5_25; - bool GX5_35; - bool GX5_45; - bool GQX_45; - bool RQX_45; - bool CXX_45; - bool CVX_10; - bool CVX_15; - bool CVX_25; - - - - - - // Update rates - int nav_rate_; - int imu_rate_; - int gps_rate_; - - clock_t start; - float field_data[3]; - float soft_iron[9]; - float soft_iron_readback[9]; - float angles[3]; - float heading_angle; - float readback_angles[3]; - float noise[3]; - float beta[3]; - float readback_beta[3]; - float readback_noise[3]; - float offset[3]; - float readback_offset[3]; - u8 com_mode; - u16 duration; - u8 reference_position_enable_command; - u8 reference_position_enable_readback; - double reference_position_command[3]; - double reference_position_readback[3]; - u8 enable_flag; - u16 estimation_control; - u16 estimation_control_readback; - u8 dynamics_mode; - u8 readback_dynamics_mode; - gx4_25_basic_status_field basic_field; - gx4_25_diagnostic_device_status_field diagnostic_field; - gx4_45_basic_status_field basic_field_45; - gx4_45_diagnostic_device_status_field diagnostic_field_45; - mip_complementary_filter_settings comp_filter_command, comp_filter_readback; - mip_filter_accel_magnitude_error_adaptive_measurement_command accel_magnitude_error_command, accel_magnitude_error_readback; - mip_filter_magnetometer_magnitude_error_adaptive_measurement_command mag_magnitude_error_command, mag_magnitude_error_readback; - mip_filter_magnetometer_dip_angle_error_adaptive_measurement_command mag_dip_angle_error_command, mag_dip_angle_error_readback; - mip_filter_zero_update_command zero_update_control, zero_update_readback; + //! @brief Reset KF service callback + bool reset_callback(std_srvs::Empty::Request &req, + std_srvs::Empty::Response &resp); + //! @brief Convience for printing packet stats + void print_packet_stats(); + + // Variables/fields + //The primary device interface structure + mip_interface device_interface_; + base_device_info_field device_info; + u8 temp_string[20]; + + //Packet Counters (valid, timeout, and checksum errors) + u32 filter_valid_packet_count_; + u32 ahrs_valid_packet_count_; + u32 gps_valid_packet_count_; + + u32 filter_timeout_packet_count_; + u32 ahrs_timeout_packet_count_; + u32 gps_timeout_packet_count_; + + u32 filter_checksum_error_packet_count_; + u32 ahrs_checksum_error_packet_count_; + u32 gps_checksum_error_packet_count_; + + //Data field storage + //AHRS + mip_ahrs_scaled_gyro curr_ahrs_gyro_; + mip_ahrs_scaled_accel curr_ahrs_accel_; + mip_ahrs_scaled_mag curr_ahrs_mag_; + mip_ahrs_quaternion curr_ahrs_quaternion_; + //GPS + mip_gps_llh_pos curr_llh_pos_; + mip_gps_ned_vel curr_ned_vel_; + mip_gps_time curr_gps_time_; + + //FILTER + mip_filter_llh_pos curr_filter_pos_; + mip_filter_ned_velocity curr_filter_vel_; + mip_filter_attitude_euler_angles curr_filter_angles_; + mip_filter_attitude_quaternion curr_filter_quaternion_; + mip_filter_compensated_angular_rate curr_filter_angular_rate_; + mip_filter_compensated_acceleration curr_filter_accel_comp_; + mip_filter_linear_acceleration curr_filter_linear_accel_; + mip_filter_llh_pos_uncertainty curr_filter_pos_uncertainty_; + mip_filter_ned_vel_uncertainty curr_filter_vel_uncertainty_; + mip_filter_euler_attitude_uncertainty curr_filter_att_uncertainty_; + mip_filter_status curr_filter_status_; + + // ROS + ros::Publisher gps_pub_; + ros::Publisher imu_pub_; + ros::Publisher filtered_imu_pub_; + ros::Publisher nav_pub_; + ros::Publisher nav_status_pub_; + ros::Publisher bias_pub_; + ros::Publisher device_status_pub_; + sensor_msgs::NavSatFix gps_msg_; + sensor_msgs::Imu imu_msg_; + sensor_msgs::Imu filtered_imu_msg_; + nav_msgs::Odometry nav_msg_; + std_msgs::Int16MultiArray nav_status_msg_; + geometry_msgs::Vector3 bias_msg_; + std::string gps_frame_id_; + std::string imu_frame_id_; + std::string odom_frame_id_; + std::string odom_child_frame_id_; + microstrain_mips::status_msg device_status_msg_; + bool publish_gps_; + bool publish_imu_; + bool publish_odom_; + bool publish_bias_; + bool publish_filtered_imu_; + bool remove_imu_gravity_; + bool frame_based_enu_; + std::vector imu_linear_cov_; + std::vector imu_angular_cov_; + std::vector imu_orientation_cov_; + + //Device Flags + bool GX5_15; + bool GX5_25; + bool GX5_35; + bool GX5_45; + bool GQX_45; + bool RQX_45; + bool CXX_45; + bool CVX_10; + bool CVX_15; + bool CVX_25; + + // Update rates + int nav_rate_; + int imu_rate_; + int gps_rate_; + + clock_t start; + float field_data[3]; + float soft_iron[9]; + float soft_iron_readback[9]; + float angles[3]; + float heading_angle; + float readback_angles[3]; + float noise[3]; + float beta[3]; + float readback_beta[3]; + float readback_noise[3]; + float offset[3]; + float readback_offset[3]; + u8 com_mode; + u16 duration; + u8 reference_position_enable_command; + u8 reference_position_enable_readback; + double reference_position_command[3]; + double reference_position_readback[3]; + u8 enable_flag; + u16 estimation_control; + u16 estimation_control_readback; + u8 dynamics_mode; + u8 readback_dynamics_mode; + gx4_25_basic_status_field basic_field; + gx4_25_diagnostic_device_status_field diagnostic_field; + gx4_45_basic_status_field basic_field_45; + gx4_45_diagnostic_device_status_field diagnostic_field_45; + mip_complementary_filter_settings comp_filter_command, comp_filter_readback; + mip_filter_accel_magnitude_error_adaptive_measurement_command accel_magnitude_error_command, accel_magnitude_error_readback; + mip_filter_magnetometer_magnitude_error_adaptive_measurement_command mag_magnitude_error_command, mag_magnitude_error_readback; + mip_filter_magnetometer_dip_angle_error_adaptive_measurement_command mag_dip_angle_error_command, mag_dip_angle_error_readback; + mip_filter_zero_update_command zero_update_control, zero_update_readback; }; // Microstrain class diff --git a/launch/microstrain.launch b/launch/microstrain.launch index a47c35f3..5c3bc98d 100644 --- a/launch/microstrain.launch +++ b/launch/microstrain.launch @@ -3,7 +3,7 @@ - + @@ -29,6 +29,12 @@ + + + @@ -37,6 +43,12 @@ + + + + + [0.01, 0, 0, 0, 0.01, 0, 0, 0, 0.01] diff --git a/src/microstrain_3dm.cpp b/src/microstrain_3dm.cpp index 4a81322c..3fd1fa15 100644 --- a/src/microstrain_3dm.cpp +++ b/src/microstrain_3dm.cpp @@ -49,6 +49,9 @@ namespace Microstrain publish_gps_(true), publish_imu_(true), publish_odom_(true), + publish_filtered_imu_(false), + remove_imu_gravity_(false), + frame_based_enu_(false), imu_linear_cov_(std::vector(9, 0.0)), imu_angular_cov_(std::vector(9, 0.0)), imu_orientation_cov_(std::vector(9, 0.0)) @@ -150,6 +153,9 @@ namespace Microstrain private_nh.param("publish_imu", publish_imu_, true); private_nh.param("publish_bias", publish_bias_, true); + private_nh.param("publish_filtered_imu", publish_filtered_imu_, false); + private_nh.param("remove_imu_gravity", remove_imu_gravity_, false); + private_nh.param("frame_based_enu", frame_based_enu_, false); // Covariance parameters to set the sensor_msg/IMU covariance values std::vector default_cov(9, 0.0); @@ -160,6 +166,8 @@ namespace Microstrain // ROS publishers and subscribers if (publish_imu_) imu_pub_ = node.advertise("imu/data", 100); + if (publish_filtered_imu_) + filtered_imu_pub_ = node.advertise("filtered/imu/data", 100); // Publishes device status device_status_pub_ = node.advertise("device/status", 100); @@ -374,6 +382,11 @@ namespace Microstrain if (publish_odom_) { nav_pub_ = node.advertise("nav/odom", 100); + } + + // This is the EKF filter status, not just navigation/odom status + if (publish_odom_ || publish_filtered_imu_) + { nav_status_pub_ = node.advertise("nav/status", 100); } @@ -414,7 +427,7 @@ namespace Microstrain // AHRS Setup // Get base rate - if (publish_imu_) + if (publish_imu_ || publish_filtered_imu_) { start = clock(); while (mip_3dm_cmd_get_ahrs_base_rate(&device_interface_, &base_rate) != MIP_INTERFACE_OK) @@ -428,8 +441,9 @@ namespace Microstrain ROS_INFO("AHRS Base Rate => %d Hz", base_rate); ros::Duration(dT).sleep(); - // Deterimine decimation to get close to goal rate - u8 imu_decimation = (u8)(static_cast(base_rate)/ static_cast(imu_rate_)); + // Deterimine decimation to get close to goal rate (We use the highest of the imu rates) + int rate = (imu_rate_ > nav_rate_) ? imu_rate_ : nav_rate_; + u8 imu_decimation = (u8)(static_cast(base_rate)/ static_cast(rate)); ROS_INFO("AHRS decimation set to %#04X", imu_decimation); // AHRS Message Format @@ -605,7 +619,7 @@ namespace Microstrain } // end of GPS setup // Filter setup - if (publish_odom_) + if (publish_odom_ || publish_filtered_imu_) { start = clock(); while (mip_3dm_cmd_get_filter_base_rate(&device_interface_, &base_rate) != MIP_INTERFACE_OK) @@ -618,30 +632,89 @@ namespace Microstrain } ROS_INFO("FILTER Base Rate => %d Hz", base_rate); - u8 nav_decimation = (u8)(static_cast(base_rate)/ static_cast(nav_rate_)); + // If we have made it this far in this statement, we know we want to publish filtered data + // from the IMU. We need to make sure to set the data rate to the correct speed dependent + // upon which filtered field we are after. Thus make sure we get the fastest data rate. + int rate = nav_rate_; + if(publish_filtered_imu_) + { + // Set filter rate based on max of filter topic rates + rate = (imu_rate_ > nav_rate_) ? imu_rate_ : nav_rate_; + } + + u8 nav_decimation = (u8)(static_cast(base_rate)/ static_cast(rate)); ros::Duration(dT).sleep(); ////////// Filter Message Format // Set ROS_INFO("Setting Filter stream format"); - data_stream_format_descriptors[0] = MIP_FILTER_DATA_LLH_POS; - data_stream_format_descriptors[1] = MIP_FILTER_DATA_NED_VEL; - // data_stream_format_descriptors[2] = MIP_FILTER_DATA_ATT_EULER_ANGLES; - data_stream_format_descriptors[2] = MIP_FILTER_DATA_ATT_QUATERNION; - data_stream_format_descriptors[3] = MIP_FILTER_DATA_POS_UNCERTAINTY; - data_stream_format_descriptors[4] = MIP_FILTER_DATA_VEL_UNCERTAINTY; - data_stream_format_descriptors[5] = MIP_FILTER_DATA_ATT_UNCERTAINTY_EULER; - data_stream_format_descriptors[6] = MIP_FILTER_DATA_COMPENSATED_ANGULAR_RATE; - data_stream_format_descriptors[7] = MIP_FILTER_DATA_FILTER_STATUS; + + // Order doesn't matter since we parse them with a case statement below. + // First start by loading the common values. + data_stream_format_descriptors[0] = MIP_FILTER_DATA_ATT_QUATERNION; + data_stream_format_descriptors[1] = MIP_FILTER_DATA_ATT_UNCERTAINTY_EULER; + data_stream_format_descriptors[2] = MIP_FILTER_DATA_COMPENSATED_ANGULAR_RATE; + data_stream_format_descriptors[3] = MIP_FILTER_DATA_FILTER_STATUS; data_stream_format_decimation[0] = nav_decimation; // 0x32; data_stream_format_decimation[1] = nav_decimation; // 0x32; data_stream_format_decimation[2] = nav_decimation; // 0x32; data_stream_format_decimation[3] = nav_decimation; // 0x32; - data_stream_format_decimation[4] = nav_decimation; // 0x32; - data_stream_format_decimation[5] = nav_decimation; // 0x32; - data_stream_format_decimation[6] = nav_decimation; // 0x32; - data_stream_format_decimation[7] = nav_decimation; // 0x32; - data_stream_format_num_entries = 8; + + // If we want the odometry add that data + if (publish_odom_ && publish_filtered_imu_) + { + // Size is up to 10 elements + data_stream_format_descriptors[4] = MIP_FILTER_DATA_LLH_POS; + data_stream_format_descriptors[5] = MIP_FILTER_DATA_NED_VEL; + // data_stream_format_descriptors[2] = MIP_FILTER_DATA_ATT_EULER_ANGLES; + data_stream_format_descriptors[6] = MIP_FILTER_DATA_POS_UNCERTAINTY; + data_stream_format_descriptors[7] = MIP_FILTER_DATA_VEL_UNCERTAINTY; + + // The filter has one message that removes gravity and one that does not + if (remove_imu_gravity_) + { + data_stream_format_descriptors[8] = MIP_FILTER_DATA_LINEAR_ACCELERATION; + } + else + { + data_stream_format_descriptors[8] = MIP_FILTER_DATA_COMPENSATED_ACCELERATION; + } + + data_stream_format_decimation[4] = nav_decimation; // 0x32; + data_stream_format_decimation[5] = nav_decimation; // 0x32; + data_stream_format_decimation[6] = nav_decimation; // 0x32; + data_stream_format_decimation[7] = nav_decimation; // 0x32; + data_stream_format_decimation[8] = nav_decimation; // 0x32; + data_stream_format_num_entries = 9; + } + else if (publish_odom_ && !publish_filtered_imu_) + { + data_stream_format_descriptors[4] = MIP_FILTER_DATA_LLH_POS; + data_stream_format_descriptors[5] = MIP_FILTER_DATA_NED_VEL; + // data_stream_format_descriptors[2] = MIP_FILTER_DATA_ATT_EULER_ANGLES; + data_stream_format_descriptors[6] = MIP_FILTER_DATA_POS_UNCERTAINTY; + data_stream_format_descriptors[7] = MIP_FILTER_DATA_VEL_UNCERTAINTY; + + data_stream_format_decimation[4] = nav_decimation; // 0x32; + data_stream_format_decimation[5] = nav_decimation; // 0x32; + data_stream_format_decimation[6] = nav_decimation; // 0x32; + data_stream_format_decimation[7] = nav_decimation; // 0x32; + data_stream_format_num_entries = 8; + } + else + { + // The filter has one message that removes gravity and one that does not + if (remove_imu_gravity_) + { + data_stream_format_descriptors[4] = MIP_FILTER_DATA_LINEAR_ACCELERATION; + } + else + { + data_stream_format_descriptors[4] = MIP_FILTER_DATA_COMPENSATED_ACCELERATION; + } + data_stream_format_decimation[4] = nav_decimation; // 0x32; + data_stream_format_num_entries = 5; + } start = clock(); while (mip_3dm_cmd_filter_message_format(&device_interface_, @@ -688,29 +761,16 @@ namespace Microstrain ros::Duration(dT).sleep(); } - // Dynamics Mode - // Set dynamics mode - ROS_INFO("Setting dynamics mode to %#04X", dynamics_mode); - start = clock(); - while (mip_filter_vehicle_dynamics_mode(&device_interface_, - MIP_FUNCTION_SELECTOR_WRITE, &dynamics_mode) != MIP_INTERFACE_OK) - { - if (clock() - start > 5000) - { - ROS_INFO("mip_filter_vehicle_dynamics_mode function timed out."); - break; - } - } - ros::Duration(dT).sleep(); - // Readback dynamics mode - if (readback_settings) + // GX5_25 doesn't appear to suport this feature thus GX5_15 probably won't either + if (GX5_35 == true || GX5_45 == true) { - // Read the settings back - ROS_INFO("Reading back dynamics mode setting"); + // Dynamics Mode + // Set dynamics mode + ROS_INFO("Setting dynamics mode to %#04X", dynamics_mode); start = clock(); while (mip_filter_vehicle_dynamics_mode(&device_interface_, - MIP_FUNCTION_SELECTOR_READ, &readback_dynamics_mode) != MIP_INTERFACE_OK) + MIP_FUNCTION_SELECTOR_WRITE, &dynamics_mode) != MIP_INTERFACE_OK) { if (clock() - start > 5000) { @@ -720,27 +780,45 @@ namespace Microstrain } ros::Duration(dT).sleep(); - if (dynamics_mode == readback_dynamics_mode) - ROS_INFO("Success: Dynamics mode setting is: %#04X", readback_dynamics_mode); - else - ROS_ERROR("Failure: Dynamics mode set to be %#04X, but reads as %#04X", - dynamics_mode, readback_dynamics_mode); - } + // Readback dynamics mode + if (readback_settings) + { + // Read the settings back + ROS_INFO("Reading back dynamics mode setting"); + start = clock(); + while (mip_filter_vehicle_dynamics_mode(&device_interface_, + MIP_FUNCTION_SELECTOR_READ, &readback_dynamics_mode) != MIP_INTERFACE_OK) + { + if (clock() - start > 5000) + { + ROS_INFO("mip_filter_vehicle_dynamics_mode function timed out."); + break; + } + } - if (save_settings) - { - ROS_INFO("Saving dynamics mode settings to EEPROM"); - start = clock(); - while (mip_filter_vehicle_dynamics_mode(&device_interface_, - MIP_FUNCTION_SELECTOR_STORE_EEPROM, NULL) != MIP_INTERFACE_OK) + ros::Duration(dT).sleep(); + if (dynamics_mode == readback_dynamics_mode) + ROS_INFO("Success: Dynamics mode setting is: %#04X", readback_dynamics_mode); + else + ROS_ERROR("Failure: Dynamics mode set to be %#04X, but reads as %#04X", + dynamics_mode, readback_dynamics_mode); + } + + if (save_settings) { - if (clock() - start > 5000) + ROS_INFO("Saving dynamics mode settings to EEPROM"); + start = clock(); + while (mip_filter_vehicle_dynamics_mode(&device_interface_, + MIP_FUNCTION_SELECTOR_STORE_EEPROM, NULL) != MIP_INTERFACE_OK) { - ROS_INFO("mip_filter_vehicle_dynamics_mode function timed out."); - break; + if (clock() - start > 5000) + { + ROS_INFO("mip_filter_vehicle_dynamics_mode function timed out."); + break; + } } + ros::Duration(dT).sleep(); } - ros::Duration(dT).sleep(); } // Set heading Source @@ -848,7 +926,7 @@ namespace Microstrain // Enable Data streams dT = 0.25; - if (publish_imu_) + if (publish_imu_ || publish_filtered_imu_) { ROS_INFO("Enabling AHRS stream"); enable = 0x01; @@ -926,6 +1004,10 @@ namespace Microstrain { max_rate = std::max(max_rate, imu_rate_); } + if (publish_filtered_imu_) + { + max_rate = std::max(std::max(max_rate, nav_rate_),imu_rate_); + } if (publish_gps_) { max_rate = std::max(max_rate, gps_rate_); @@ -3148,7 +3230,7 @@ namespace Microstrain u16 field_offset = 0; // If we aren't publishing, then return - if (!publish_odom_) + if (!publish_odom_ && !publish_filtered_imu_) return; // ROS_INFO("Filter callback"); @@ -3245,11 +3327,39 @@ namespace Microstrain // For little-endian targets, byteswap the data field mip_filter_attitude_quaternion_byteswap(&curr_filter_quaternion_); - // put into ENU - swap X/Y, invert Z - nav_msg_.pose.pose.orientation.x = curr_filter_quaternion_.q[2]; - nav_msg_.pose.pose.orientation.y = curr_filter_quaternion_.q[1]; - nav_msg_.pose.pose.orientation.z = -1.0*curr_filter_quaternion_.q[3]; - nav_msg_.pose.pose.orientation.w = curr_filter_quaternion_.q[0]; + // If we want the orientation to be based on the reference on the imu + tf2::Quaternion q(curr_filter_quaternion_.q[1],curr_filter_quaternion_.q[2], + curr_filter_quaternion_.q[3],curr_filter_quaternion_.q[0]); + geometry_msgs::Quaternion quat_msg; + + if(frame_based_enu_) + { + // Create a rotation from NED -> ENU + tf2::Quaternion q_rotate; + q_rotate.setRPY(M_PI,0.0,M_PI/2); + // Apply the NED to ENU rotation such that the coordinate frame matches + q = q_rotate*q; + quat_msg = tf2::toMsg(q); + } + else + { + // put into ENU - swap X/Y, invert Z + quat_msg.x = q[1]; + quat_msg.y = q[0]; + quat_msg.z = -1.0*q[2]; + quat_msg.w = q[3]; + } + + nav_msg_.pose.pose.orientation = quat_msg; + + if (publish_filtered_imu_) + { + // Header + filtered_imu_msg_.header.seq = filter_valid_packet_count_; + filtered_imu_msg_.header.stamp = ros::Time::now(); + filtered_imu_msg_.header.frame_id = imu_frame_id_; + filtered_imu_msg_.orientation = nav_msg_.pose.pose.orientation; + } } break; @@ -3264,6 +3374,13 @@ namespace Microstrain nav_msg_.twist.twist.angular.x = curr_filter_angular_rate_.x; nav_msg_.twist.twist.angular.y = curr_filter_angular_rate_.y; nav_msg_.twist.twist.angular.z = curr_filter_angular_rate_.z; + + if (publish_filtered_imu_) + { + filtered_imu_msg_.angular_velocity.x = curr_filter_angular_rate_.x; + filtered_imu_msg_.angular_velocity.y = curr_filter_angular_rate_.y; + filtered_imu_msg_.angular_velocity.z = curr_filter_angular_rate_.z; + } } break; @@ -3306,6 +3423,16 @@ namespace Microstrain nav_msg_.pose.covariance[21] = curr_filter_att_uncertainty_.roll*curr_filter_att_uncertainty_.roll; nav_msg_.pose.covariance[28] = curr_filter_att_uncertainty_.pitch*curr_filter_att_uncertainty_.pitch; nav_msg_.pose.covariance[35] = curr_filter_att_uncertainty_.yaw*curr_filter_att_uncertainty_.yaw; + + if (publish_filtered_imu_) + { + filtered_imu_msg_.orientation_covariance[0] = + curr_filter_att_uncertainty_.roll*curr_filter_att_uncertainty_.roll; + filtered_imu_msg_.orientation_covariance[4] = + curr_filter_att_uncertainty_.pitch*curr_filter_att_uncertainty_.pitch; + filtered_imu_msg_.orientation_covariance[8] = + curr_filter_att_uncertainty_.yaw*curr_filter_att_uncertainty_.yaw; + } } break; @@ -3329,12 +3456,71 @@ namespace Microstrain } break; + /// + // Scaled Accelerometer + /// + + case MIP_FILTER_DATA_LINEAR_ACCELERATION: + { + memcpy(&curr_filter_linear_accel_, field_data, sizeof(mip_filter_linear_acceleration)); + + // For little-endian targets, byteswap the data field + mip_filter_linear_acceleration_byteswap(&curr_filter_linear_accel_); + + // If we want gravity removed, use this as acceleration + if (remove_imu_gravity_) + { + // Stuff into ROS message - acceleration already in m/s^2 + filtered_imu_msg_.linear_acceleration.x = curr_filter_linear_accel_.x; + filtered_imu_msg_.linear_acceleration.y = curr_filter_linear_accel_.y; + filtered_imu_msg_.linear_acceleration.z = curr_filter_linear_accel_.z; + } + // Otherwise, do nothing with this packet + } + break; + + case MIP_FILTER_DATA_COMPENSATED_ACCELERATION: + { + memcpy(&curr_filter_accel_comp_, field_data, sizeof(mip_filter_compensated_acceleration)); + + // For little-endian targets, byteswap the data field + mip_filter_compensated_acceleration_byteswap(&curr_filter_accel_comp_); + + // If we do not want to have gravity removed, use this as acceleration + if (!remove_imu_gravity_) + { + // Stuff into ROS message - acceleration already in m/s^2 + filtered_imu_msg_.linear_acceleration.x = curr_filter_accel_comp_.x; + filtered_imu_msg_.linear_acceleration.y = curr_filter_accel_comp_.y; + filtered_imu_msg_.linear_acceleration.z = curr_filter_accel_comp_.z; + } + // Otherwise, do nothing with this packet + } + break; + default: break; } } // Publish - nav_pub_.publish(nav_msg_); + if (publish_odom_) + { + nav_pub_.publish(nav_msg_); + } + + if (publish_filtered_imu_) + { + // Does it make sense to get the angular velocity bias and acceleration bias to populate these? + // Since the sensor does not produce a covariance for linear acceleration, set it based + // on our pulled in parameters. + std::copy(imu_linear_cov_.begin(), imu_linear_cov_.end(), + filtered_imu_msg_.linear_acceleration_covariance.begin()); + // Since the sensor does not produce a covariance for angular velocity, set it based + // on our pulled in parameters. + std::copy(imu_angular_cov_.begin(), imu_angular_cov_.end(), + filtered_imu_msg_.angular_velocity_covariance.begin()); + filtered_imu_pub_.publish(filtered_imu_msg_); + } } break; @@ -3515,11 +3701,34 @@ namespace Microstrain // For little-endian targets, byteswap the data field mip_ahrs_quaternion_byteswap(&curr_ahrs_quaternion_); - // put into ENU - swap X/Y, invert Z - imu_msg_.orientation.x = curr_ahrs_quaternion_.q[2]; - imu_msg_.orientation.y = curr_ahrs_quaternion_.q[1]; - imu_msg_.orientation.z = -1.0*curr_ahrs_quaternion_.q[3]; - imu_msg_.orientation.w = curr_ahrs_quaternion_.q[0]; + + // If we want the orientation to be based on the reference on the imu + tf2::Quaternion q(curr_ahrs_quaternion_.q[1],curr_ahrs_quaternion_.q[2], + curr_ahrs_quaternion_.q[3],curr_ahrs_quaternion_.q[0]); + geometry_msgs::Quaternion quat_msg; + + if(frame_based_enu_) + { + // Create a rotation from NED -> ENU + tf2::Quaternion q_rotate; + q_rotate.setRPY(M_PI,0.0,M_PI/2); + // Apply the NED to ENU rotation such that the coordinate frame matches + q = q_rotate*q; + quat_msg = tf2::toMsg(q); + } + else + { + // put into ENU - swap X/Y, invert Z + quat_msg.x = q[1]; + quat_msg.y = q[0]; + quat_msg.z = -1.0*q[2]; + quat_msg.w = q[3]; + } + + imu_msg_.orientation = quat_msg; + + + // Since the MIP_AHRS data does not contain uncertainty values // we have to set them based on the parameter values. std::copy(imu_orientation_cov_.begin(), imu_orientation_cov_.end(),