diff --git a/odom_estimator/CMakeLists.txt b/odom_estimator/CMakeLists.txt index 6843160..d6343ad 100644 --- a/odom_estimator/CMakeLists.txt +++ b/odom_estimator/CMakeLists.txt @@ -4,7 +4,7 @@ cmake_minimum_required(VERSION 2.8.3) project(odom_estimator) # Load catkin and all dependencies required for this package # TODO: remove all from COMPONENTS that are not catkin packages. -find_package(catkin REQUIRED COMPONENTS roscpp pluginlib sensor_msgs eigen_conversions geometry_msgs tf nodelet tf_conversions std_msgs message_generation cmake_modules uf_common) +find_package(catkin REQUIRED COMPONENTS roscpp pluginlib sensor_msgs eigen_conversions geometry_msgs tf nodelet tf_conversions std_msgs message_generation cmake_modules mil_msgs) # CATKIN_MIGRATION: removed during catkin migration # cmake_minimum_required(VERSION 2.4.6) @@ -55,7 +55,7 @@ generate_messages( # TODO: fill in what other packages will need to use this package catkin_package( DEPENDS Eigen # TODO - CATKIN_DEPENDS roscpp pluginlib sensor_msgs eigen_conversions geometry_msgs tf uf_common nodelet tf_conversions std_msgs message_runtime + CATKIN_DEPENDS roscpp pluginlib sensor_msgs eigen_conversions geometry_msgs tf mil_msgs nodelet tf_conversions std_msgs message_runtime INCLUDE_DIRS ${EIGEN_INCLUDE_DIRS} include# TODO include LIBRARIES # TODO ) diff --git a/odom_estimator/package.xml b/odom_estimator/package.xml index d9f30f8..0f97b59 100644 --- a/odom_estimator/package.xml +++ b/odom_estimator/package.xml @@ -26,7 +26,7 @@ std_msgs message_generation cmake_modules - uf_common + mil_msgs roscpp @@ -39,7 +39,7 @@ tf_conversions std_msgs message_runtime - uf_common + mil_msgs diff --git a/odom_estimator/src/nodelet.cpp b/odom_estimator/src/nodelet.cpp index d87c8b4..9044ac3 100644 --- a/odom_estimator/src/nodelet.cpp +++ b/odom_estimator/src/nodelet.cpp @@ -13,8 +13,8 @@ #include #include -#include -#include +#include +#include #include #include @@ -263,8 +263,8 @@ class NodeImpl { *state); } - void got_dvl(const uf_common::VelocityMeasurementsConstPtr &msgp) { - uf_common::VelocityMeasurements const & msg = *msgp; + void got_dvl(const mil_msgs::VelocityMeasurementsConstPtr &msgp) { + mil_msgs::VelocityMeasurements const & msg = *msgp; tf::StampedTransform transform; try { @@ -277,9 +277,9 @@ class NodeImpl { Vec<3> local_dvl_pos; tf::vectorTFToEigen(transform.getOrigin(), local_dvl_pos); Quaternion local_dvl_orientation; tf::quaternionTFToEigen(transform.getRotation(), local_dvl_orientation); - std::vector good; + std::vector good; for(unsigned int i = 0; i < msg.velocity_measurements.size(); i++) { - uf_common::VelocityMeasurement const & vm = msg.velocity_measurements[i]; + mil_msgs::VelocityMeasurement const & vm = msg.velocity_measurements[i]; if(!std::isnan(vm.velocity)) { good.push_back(vm); } @@ -303,7 +303,7 @@ class NodeImpl { Vec res(good.size()); for(unsigned int i = 0; i < good.size(); i++) { - uf_common::VelocityMeasurement const & vm = good[i]; + mil_msgs::VelocityMeasurement const & vm = good[i]; res(i) = (xyz2vec(vm.direction).dot(dvl_vel) + measurement_noise(i)) - vm.velocity; } return res; @@ -314,8 +314,8 @@ class NodeImpl { *state); } - void got_depth(const uf_common::Float64StampedConstPtr &msgp) { - uf_common::Float64Stamped const & msg = *msgp; + void got_depth(const mil_msgs::DepthStampedConstPtr &msgp) { + mil_msgs::DepthStamped const & msg = *msgp; tf::StampedTransform transform; @@ -335,7 +335,7 @@ class NodeImpl { [&](State const &state, Vec<1> const &measurement_noise) { SqMat<3> m = enu_from_ecef_mat(state.getPosECEF()); double estimated = -(m * state.getRelPosECEF(local_depth_pos))(2) + measurement_noise(0); - return scalar_matrix(estimated - msg.data); + return scalar_matrix(estimated - msg.depth); }, GaussianDistribution >( Vec<1>::Zero(), @@ -354,10 +354,10 @@ class NodeImpl { ros::Subscriber imu_sub; message_filters::Subscriber mag_sub; tf::MessageFilter mag_filter; - message_filters::Subscriber dvl_sub; - tf::MessageFilter dvl_filter; - message_filters::Subscriber depth_sub; - tf::MessageFilter depth_filter; + message_filters::Subscriber dvl_sub; + tf::MessageFilter dvl_filter; + message_filters::Subscriber depth_sub; + tf::MessageFilter depth_filter; ros::Publisher odom_pub; ros::Publisher absodom_pub; ros::Publisher info_pub;