diff --git a/CMakeLists.txt b/CMakeLists.txt index f9116088..ffb00af8 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -62,12 +62,6 @@ roslint_cpp() ################################### ## catkin specific configuration ## ################################### -# Generate messages in the 'msg' folder -add_message_files( - DIRECTORY msg - FILES - ImuBiasValidity.msg -) add_service_files( FILES diff --git a/include/robot_localization/ros_filter.h b/include/robot_localization/ros_filter.h index 6d3d1912..9a012003 100644 --- a/include/robot_localization/ros_filter.h +++ b/include/robot_localization/ros_filter.h @@ -40,7 +40,6 @@ #include #include -#include #include #include diff --git a/msg/ImuBiasValidity.msg b/msg/ImuBiasValidity.msg deleted file mode 100644 index eda371a7..00000000 --- a/msg/ImuBiasValidity.msg +++ /dev/null @@ -1,3 +0,0 @@ -bool roll -bool pitch -bool yaw diff --git a/src/ros_filter.cpp b/src/ros_filter.cpp index c3fa818c..af2bba76 100644 --- a/src/ros_filter.cpp +++ b/src/ros_filter.cpp @@ -1905,7 +1905,9 @@ namespace RobotLocalization boost::bind(&RosFilter::imuMagnetometerValidityCallback, this, _1, dynamic_correction_topic), ros::VoidPtr(), ros::TransportHints().tcpNoDelay(nodelayImu))); - imuDataValidityPubMap_[dynamic_correction_topic] = nhLocal_.advertise(dynamic_correction_topic+"/bias_estimator_validity", 20); + imuDataValidityPubMap_[dynamic_correction_topic + "_roll"] = nhLocal_.advertise(dynamic_correction_topic + "/bias_estimator_validity_roll", 20); + imuDataValidityPubMap_[dynamic_correction_topic + "_pitch"] = nhLocal_.advertise(dynamic_correction_topic + "/bias_estimator_validity_pitch", 20); + imuDataValidityPubMap_[dynamic_correction_topic + "_yaw"] = nhLocal_.advertise(dynamic_correction_topic + "/bias_estimator_validity_yaw", 20); // Subscribe to resets as well topicSubs_.push_back( @@ -3494,11 +3496,15 @@ namespace RobotLocalization } // publish validity of axis based on whether bias estimator updated the bias - robot_localization::ImuBiasValidity estimator_validity_msg; - estimator_validity_msg.roll = is_bias_valid[0]; - estimator_validity_msg.pitch = is_bias_valid[1]; - estimator_validity_msg.yaw = is_bias_valid[2]; - imuDataValidityPubMap_[topicName].publish(estimator_validity_msg); + std_msgs::Bool estimator_validity_msg; + estimator_validity_msg.data = is_bias_valid[0]; + imuDataValidityPubMap_[topicName + "_roll"].publish(estimator_validity_msg); + + estimator_validity_msg.data = is_bias_valid[1]; + imuDataValidityPubMap_[topicName + "_pitch"].publish(estimator_validity_msg); + + estimator_validity_msg.data = is_bias_valid[2]; + imuDataValidityPubMap_[topicName + "_yaw"].publish(estimator_validity_msg); if(can_update == true) {