From e5df149d93995d944e8d7f61fabb24305ac3e9bc Mon Sep 17 00:00:00 2001 From: Kevin Allen Date: Mon, 10 Apr 2017 23:54:17 -0400 Subject: [PATCH] DEPENDENCIES: replace deprecated Float64Stamped w/ DepthStamped --- odom_estimator/src/nodelet.cpp | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/odom_estimator/src/nodelet.cpp b/odom_estimator/src/nodelet.cpp index 091de85..9044ac3 100644 --- a/odom_estimator/src/nodelet.cpp +++ b/odom_estimator/src/nodelet.cpp @@ -14,7 +14,7 @@ #include #include -#include +#include #include #include @@ -314,8 +314,8 @@ class NodeImpl { *state); } - void got_depth(const mil_msgs::Float64StampedConstPtr &msgp) { - mil_msgs::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(), @@ -356,8 +356,8 @@ class NodeImpl { 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 depth_sub; + tf::MessageFilter depth_filter; ros::Publisher odom_pub; ros::Publisher absodom_pub; ros::Publisher info_pub;