From 20d25e088aaf25c14dd3442475c48fc7837776c6 Mon Sep 17 00:00:00 2001 From: Sam Date: Tue, 16 Jul 2019 10:37:01 -0400 Subject: [PATCH 01/10] Added filter message publication from the GX5-25. Did a little more cleanup in the header file. --- include/microstrain_mips/microstrain_3dm.h | 260 ++++++++++---------- launch/microstrain.launch | 8 +- src/microstrain_3dm.cpp | 265 ++++++++++++++++----- 3 files changed, 343 insertions(+), 190 deletions(-) diff --git a/include/microstrain_mips/microstrain_3dm.h b/include/microstrain_mips/microstrain_3dm.h index 94333c04..9c2cdc0c 100644 --- a/include/microstrain_mips/microstrain_3dm.h +++ b/include/microstrain_mips/microstrain_3dm.h @@ -42,7 +42,6 @@ extern "C" #include #include - // ROS #include "ros/ros.h" #include "sensor_msgs/NavSatFix.h" @@ -82,7 +81,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 +96,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 +236,134 @@ 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_; + 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..0753d9b3 100644 --- a/launch/microstrain.launch +++ b/launch/microstrain.launch @@ -3,7 +3,7 @@ - + @@ -37,6 +37,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..fcbc4b4c 100644 --- a/src/microstrain_3dm.cpp +++ b/src/microstrain_3dm.cpp @@ -49,6 +49,8 @@ namespace Microstrain publish_gps_(true), publish_imu_(true), publish_odom_(true), + publish_filtered_imu_(false), + remove_imu_gravity_(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 +152,8 @@ 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); // Covariance parameters to set the sensor_msg/IMU covariance values std::vector default_cov(9, 0.0); @@ -160,6 +164,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,7 +380,12 @@ namespace Microstrain if (publish_odom_) { nav_pub_ = node.advertise("nav/odom", 100); - nav_status_pub_ = node.advertise("nav/status", 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); } // Setup device callbacks @@ -605,7 +616,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) @@ -624,24 +635,74 @@ namespace Microstrain ////////// 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; - 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; + + // 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; + + // 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 +749,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 +768,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 @@ -3148,7 +3214,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"); @@ -3250,6 +3316,18 @@ namespace Microstrain 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 (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.x = curr_filter_quaternion_.q[2]; + filtered_imu_msg_.orientation.y = curr_filter_quaternion_.q[1]; + filtered_imu_msg_.orientation.z = -1.0*curr_filter_quaternion_.q[3]; + filtered_imu_msg_.orientation.w = curr_filter_quaternion_.q[0]; + } } break; @@ -3264,6 +3342,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 +3391,13 @@ 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 +3421,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_) + { + // TODO: 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; From e201abbad6d5b56ac723a2af39da807abbdc74f5 Mon Sep 17 00:00:00 2001 From: Sam Date: Tue, 16 Jul 2019 10:55:51 -0400 Subject: [PATCH 02/10] Last little bit of cleanup to make roslint happy. --- src/microstrain_3dm.cpp | 75 +++++++++++++++++++++-------------------- 1 file changed, 39 insertions(+), 36 deletions(-) diff --git a/src/microstrain_3dm.cpp b/src/microstrain_3dm.cpp index fcbc4b4c..417571ec 100644 --- a/src/microstrain_3dm.cpp +++ b/src/microstrain_3dm.cpp @@ -152,8 +152,8 @@ 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("publish_filtered_imu", publish_filtered_imu_, false); + private_nh.param("remove_imu_gravity", remove_imu_gravity_, false); // Covariance parameters to set the sensor_msg/IMU covariance values std::vector default_cov(9, 0.0); @@ -165,7 +165,7 @@ namespace Microstrain if (publish_imu_) imu_pub_ = node.advertise("imu/data", 100); if (publish_filtered_imu_) - filtered_imu_pub_ = node.advertise("filtered/imu/data",100); + filtered_imu_pub_ = node.advertise("filtered/imu/data", 100); // Publishes device status device_status_pub_ = node.advertise("device/status", 100); @@ -384,8 +384,8 @@ namespace Microstrain // 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); + { + nav_status_pub_ = node.advertise("nav/status", 100); } // Setup device callbacks @@ -642,10 +642,10 @@ namespace Microstrain 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[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; // If we want the odometry add that data if (publish_odom_ && publish_filtered_imu_) @@ -653,7 +653,7 @@ namespace Microstrain // 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[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; @@ -667,30 +667,29 @@ namespace Microstrain 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_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[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_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_) { @@ -700,7 +699,7 @@ namespace Microstrain { data_stream_format_descriptors[4] = MIP_FILTER_DATA_COMPENSATED_ACCELERATION; } - data_stream_format_decimation[4] = nav_decimation; //0x32; + data_stream_format_decimation[4] = nav_decimation; // 0x32; data_stream_format_num_entries = 5; } @@ -751,7 +750,7 @@ namespace Microstrain // GX5_25 doesn't appear to suport this feature thus GX5_15 probably won't either - if(GX5_35 == true || GX5_45 == true) + if (GX5_35 == true || GX5_45 == true) { // Dynamics Mode // Set dynamics mode @@ -3316,7 +3315,7 @@ namespace Microstrain 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 (publish_filtered_imu_) { // Header @@ -3342,7 +3341,7 @@ 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; @@ -3391,12 +3390,15 @@ 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; + 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; @@ -3429,7 +3431,7 @@ namespace Microstrain { memcpy(&curr_filter_linear_accel_, field_data, sizeof(mip_filter_linear_acceleration)); - //For little-endian targets, byteswap the data field + // 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 @@ -3446,10 +3448,9 @@ namespace Microstrain 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 + // 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 @@ -3476,13 +3477,15 @@ namespace Microstrain if (publish_filtered_imu_) { - // TODO: Does it make sense to get the angular velocity bias and acceleration bias to populate these? + // 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()); + 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()); + std::copy(imu_angular_cov_.begin(), imu_angular_cov_.end(), + filtered_imu_msg_.angular_velocity_covariance.begin()); filtered_imu_pub_.publish(filtered_imu_msg_); } From 6d4c88857a695527c725b63e6f879649b32cb551 Mon Sep 17 00:00:00 2001 From: Sam Date: Thu, 25 Jul 2019 10:48:02 -0400 Subject: [PATCH 03/10] Updated filtered IMU publish rate so that if we publish filtered IMU messages we take the higher of the two rates. --- src/microstrain_3dm.cpp | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) diff --git a/src/microstrain_3dm.cpp b/src/microstrain_3dm.cpp index 417571ec..681cc1f5 100644 --- a/src/microstrain_3dm.cpp +++ b/src/microstrain_3dm.cpp @@ -425,7 +425,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) @@ -439,8 +439,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 From 64f636d2f6b9fcb1f768a8948ff2f3774b30f684 Mon Sep 17 00:00:00 2001 From: Sam Date: Thu, 25 Jul 2019 10:53:29 -0400 Subject: [PATCH 04/10] Added in a check if we are publishing filtered data rate. --- src/microstrain_3dm.cpp | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/src/microstrain_3dm.cpp b/src/microstrain_3dm.cpp index 681cc1f5..b11c8046 100644 --- a/src/microstrain_3dm.cpp +++ b/src/microstrain_3dm.cpp @@ -992,6 +992,10 @@ namespace Microstrain { max_rate = std::max(max_rate, imu_rate_); } + if (publish_filtered_imu_) + { + max_rate = std::max(max_rate, nav_rate_); + } if (publish_gps_) { max_rate = std::max(max_rate, gps_rate_); From d2ba7a66a21d20a6801fa4c21d115fa4e0eecc34 Mon Sep 17 00:00:00 2001 From: Sam Date: Thu, 15 Aug 2019 09:05:02 -0400 Subject: [PATCH 05/10] Updated driver so that it publishes properly dependent upon which IMU data the user is after. AHRS streams need to be enabled for both cases. --- launch/microstrain.launch | 2 +- src/microstrain_3dm.cpp | 16 +++++++++++++--- 2 files changed, 14 insertions(+), 4 deletions(-) diff --git a/launch/microstrain.launch b/launch/microstrain.launch index 0753d9b3..8e41ea6a 100644 --- a/launch/microstrain.launch +++ b/launch/microstrain.launch @@ -40,7 +40,7 @@ - + diff --git a/src/microstrain_3dm.cpp b/src/microstrain_3dm.cpp index b11c8046..54ac3516 100644 --- a/src/microstrain_3dm.cpp +++ b/src/microstrain_3dm.cpp @@ -630,7 +630,17 @@ 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 @@ -914,7 +924,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; @@ -994,7 +1004,7 @@ namespace Microstrain } if (publish_filtered_imu_) { - max_rate = std::max(max_rate, nav_rate_); + max_rate = std::max(std::max(max_rate, nav_rate_),imu_rate_); } if (publish_gps_) { From df4cfe3ab368f67586af40625be5da382484dc5d Mon Sep 17 00:00:00 2001 From: Sam Date: Thu, 15 Aug 2019 09:52:21 -0400 Subject: [PATCH 06/10] Updated ENU NED swap to match device labeling. --- include/microstrain_mips/microstrain_3dm.h | 1 + src/microstrain_3dm.cpp | 69 +++++++++++++++++----- 2 files changed, 55 insertions(+), 15 deletions(-) diff --git a/include/microstrain_mips/microstrain_3dm.h b/include/microstrain_mips/microstrain_3dm.h index 9c2cdc0c..c717320f 100644 --- a/include/microstrain_mips/microstrain_3dm.h +++ b/include/microstrain_mips/microstrain_3dm.h @@ -310,6 +310,7 @@ namespace Microstrain 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_; diff --git a/src/microstrain_3dm.cpp b/src/microstrain_3dm.cpp index 54ac3516..e55229dc 100644 --- a/src/microstrain_3dm.cpp +++ b/src/microstrain_3dm.cpp @@ -51,6 +51,7 @@ namespace Microstrain 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)) @@ -3325,11 +3326,30 @@ 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::Qauternion 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::Qauternion 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[2]; + quat_msg.y = q[1]; + quat_msg.z = -1.0*q[3]; + quat_msg.w = q[0]; + } + + nav_msg_.pose.pose.orientation = quat_msg; if (publish_filtered_imu_) { @@ -3337,10 +3357,7 @@ namespace Microstrain 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.x = curr_filter_quaternion_.q[2]; - filtered_imu_msg_.orientation.y = curr_filter_quaternion_.q[1]; - filtered_imu_msg_.orientation.z = -1.0*curr_filter_quaternion_.q[3]; - filtered_imu_msg_.orientation.w = curr_filter_quaternion_.q[0]; + filtered_imu_msg_.orientation = nav_msg_.pose.pose.orientation; } } break; @@ -3501,7 +3518,6 @@ namespace Microstrain // 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_); } } @@ -3684,11 +3700,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::Qauternion 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::Qauternion 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[2]; + quat_msg.y = q[1]; + quat_msg.z = -1.0*q[3]; + quat_msg.w = q[0]; + } + + 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(), From 03398c817df2f31867e732a5713b698bfc0aecbd Mon Sep 17 00:00:00 2001 From: Sam Date: Thu, 15 Aug 2019 09:57:21 -0400 Subject: [PATCH 07/10] Typo fix... :) --- src/microstrain_3dm.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/src/microstrain_3dm.cpp b/src/microstrain_3dm.cpp index e55229dc..e4d103f0 100644 --- a/src/microstrain_3dm.cpp +++ b/src/microstrain_3dm.cpp @@ -3327,14 +3327,14 @@ namespace Microstrain mip_filter_attitude_quaternion_byteswap(&curr_filter_quaternion_); // If we want the orientation to be based on the reference on the imu - tf2::Qauternion q(curr_filter_quaternion_.q[1],curr_filter_quaternion_.q[2], + 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::Qauternion q_rotate; + 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; @@ -3702,14 +3702,14 @@ namespace Microstrain mip_ahrs_quaternion_byteswap(&curr_ahrs_quaternion_); // If we want the orientation to be based on the reference on the imu - tf2::Qauternion q(curr_ahrs_quaternion_.q[1],curr_ahrs_quaternion_.q[2], + 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::Qauternion q_rotate; + 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; From 000248408a459ddd98263df80ccf5e616d8a887c Mon Sep 17 00:00:00 2001 From: Sam Date: Thu, 15 Aug 2019 10:00:04 -0400 Subject: [PATCH 08/10] Added in the tf2_geometry_msgs so that the toMsg function is available. --- include/microstrain_mips/microstrain_3dm.h | 1 + 1 file changed, 1 insertion(+) diff --git a/include/microstrain_mips/microstrain_3dm.h b/include/microstrain_mips/microstrain_3dm.h index c717320f..f5215b81 100644 --- a/include/microstrain_mips/microstrain_3dm.h +++ b/include/microstrain_mips/microstrain_3dm.h @@ -47,6 +47,7 @@ extern "C" #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" From 7254cba8c43db9fa9ff4f557b17a6a953a12e7b1 Mon Sep 17 00:00:00 2001 From: Sam Date: Thu, 15 Aug 2019 10:05:37 -0400 Subject: [PATCH 09/10] Forgot to expose the parameter for the ENU -> NED swap previously. Done now. --- launch/microstrain.launch | 6 ++++++ src/microstrain_3dm.cpp | 1 + 2 files changed, 7 insertions(+) diff --git a/launch/microstrain.launch b/launch/microstrain.launch index 8e41ea6a..5c3bc98d 100644 --- a/launch/microstrain.launch +++ b/launch/microstrain.launch @@ -29,6 +29,12 @@ + + + diff --git a/src/microstrain_3dm.cpp b/src/microstrain_3dm.cpp index e4d103f0..b2104a91 100644 --- a/src/microstrain_3dm.cpp +++ b/src/microstrain_3dm.cpp @@ -155,6 +155,7 @@ namespace Microstrain 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); From 98c79a9f923b0adaa72481cd93171c07faf530e7 Mon Sep 17 00:00:00 2001 From: Sam Date: Thu, 15 Aug 2019 10:19:57 -0400 Subject: [PATCH 10/10] Made sure that in the original setting it defualts to the proper X & Y swapped with Z inverted. --- src/microstrain_3dm.cpp | 16 ++++++++-------- 1 file changed, 8 insertions(+), 8 deletions(-) diff --git a/src/microstrain_3dm.cpp b/src/microstrain_3dm.cpp index b2104a91..3fd1fa15 100644 --- a/src/microstrain_3dm.cpp +++ b/src/microstrain_3dm.cpp @@ -3344,10 +3344,10 @@ namespace Microstrain else { // put into ENU - swap X/Y, invert Z - quat_msg.x = q[2]; - quat_msg.y = q[1]; - quat_msg.z = -1.0*q[3]; - quat_msg.w = q[0]; + 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; @@ -3719,10 +3719,10 @@ namespace Microstrain else { // put into ENU - swap X/Y, invert Z - quat_msg.x = q[2]; - quat_msg.y = q[1]; - quat_msg.z = -1.0*q[3]; - quat_msg.w = q[0]; + 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;