diff --git a/.gitignore b/.gitignore new file mode 100644 index 0000000..a3062be --- /dev/null +++ b/.gitignore @@ -0,0 +1 @@ +.vscode/* diff --git a/CMakeLists.txt b/CMakeLists.txt index a2d8529..4ae46c1 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -11,7 +11,9 @@ link_directories(${BFL_LIBRARY_DIRS}) find_package(catkin REQUIRED COMPONENTS roscpp - tf + tf2 + tf2_ros + tf2_geometry_msgs nav_msgs std_msgs geometry_msgs @@ -50,15 +52,18 @@ include_directories( ) add_executable(robot_pose_ekf - src/odom_estimation.cpp - src/nonlinearanalyticconditionalgaussianodo.cpp - src/odom_estimation_node.cpp) + src/odom_estimation.cpp + src/nonlinearanalyticconditionalgaussianodo.cpp + src/odom_estimation_node.cpp +) target_link_libraries(robot_pose_ekf ${catkin_LIBRARIES} ${Boost_LIBRARIES} ${BFL_LIBRARIES} ) -add_dependencies(robot_pose_ekf ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) +add_dependencies(robot_pose_ekf + ${${PROJECT_NAME}_EXPORTED_TARGETS} + ${catkin_EXPORTED_TARGETS}) install( TARGETS diff --git a/include/robot_pose_ekf/odom_estimation.h b/include/robot_pose_ekf/odom_estimation.h index ba976b0..b970f87 100644 --- a/include/robot_pose_ekf/odom_estimation.h +++ b/include/robot_pose_ekf/odom_estimation.h @@ -45,14 +45,21 @@ #include #include "nonlinearanalyticconditionalgaussianodo.h" -// TF -#include +// TF2 +#include +#include +#include +#include +#include +#include // msgs #include +#include // log files #include +#include namespace estimation { @@ -61,7 +68,9 @@ class OdomEstimation { public: /// constructor - OdomEstimation(); + OdomEstimation( + std::shared_ptr tf_buffer, + std::shared_ptr tf_listener); /// destructor virtual ~OdomEstimation(); @@ -81,7 +90,7 @@ class OdomEstimation * \param prior the prior robot pose * \param time the initial time of the ekf */ - void initialize(const tf::Transform& prior, const ros::Time& time); + void initialize(const tf2::Transform& prior, const ros::Time& time); /** check if the filter is initialized * returns true if the ekf has been initialized already @@ -97,13 +106,13 @@ class OdomEstimation * \param time the time of the filter posterior * \param estimate the filter posterior as a tf transform */ - void getEstimate(ros::Time time, tf::Transform& estimate); + void getEstimate(ros::Time time, tf2::Transform& estimate); /** get the filter posterior * \param time the time of the filter posterior * \param estimate the filter posterior as a stamped tf transform */ - void getEstimate(ros::Time time, tf::StampedTransform& estimate); + void getEstimate(ros::Time time, geometry_msgs::TransformStamped& estimate); /** get the filter posterior * \param estimate the filter posterior as a pose with covariance @@ -113,32 +122,30 @@ class OdomEstimation /** Add a sensor measurement to the measurement buffer * \param meas the measurement to add */ - void addMeasurement(const tf::StampedTransform& meas); + void addMeasurement(const geometry_msgs::TransformStamped& meas); /** Add a sensor measurement to the measurement buffer * \param meas the measurement to add * \param covar the 6x6 covariance matrix of this measurement, as defined in the PoseWithCovariance message */ - void addMeasurement(const tf::StampedTransform& meas, const MatrixWrapper::SymmetricMatrix& covar); - - /** set the output frame used by tf - * \param output_frame the desired output frame published on /tf (e.g., odom_combined) - */ - void setOutputFrame(const std::string& output_frame); + void addMeasurement(const geometry_msgs::TransformStamped& meas, const MatrixWrapper::SymmetricMatrix& covar); /** set the base_footprint frame of the robot used by tf * \param base_frame the desired base frame from which to transform when publishing the combined odometry frame (e.g., base_footprint) */ void setBaseFootprintFrame(const std::string& base_frame); + void setOutputFrame(const std::string& output_frame); + + private: /// correct for angle overflow void angleOverflowCorrect(double& a, double ref); // decompose Transform into x,y,z,Rx,Ry,Rz - void decomposeTransform(const tf::StampedTransform& trans, + void decomposeTransform(const geometry_msgs::TransformStamped& trans, double& x, double& y, double&z, double&Rx, double& Ry, double& Rz); - void decomposeTransform(const tf::Transform& trans, + void decomposeTransform(const tf2::Transform& trans, double& x, double& y, double&z, double&Rx, double& Ry, double& Rz); @@ -159,8 +166,8 @@ class OdomEstimation // vars MatrixWrapper::ColumnVector vel_desi_, filter_estimate_old_vec_; - tf::Transform filter_estimate_old_; - tf::StampedTransform odom_meas_, odom_meas_old_, imu_meas_, imu_meas_old_, vo_meas_, vo_meas_old_, gps_meas_, gps_meas_old_; + tf2::Transform filter_estimate_old_; + geometry_msgs::TransformStamped odom_meas_, odom_meas_old_, imu_meas_, imu_meas_old_, vo_meas_, vo_meas_old_, gps_meas_, gps_meas_old_; ros::Time filter_time_old_; bool filter_initialized_, odom_initialized_, imu_initialized_, vo_initialized_, gps_initialized_; @@ -168,11 +175,24 @@ class OdomEstimation double diagnostics_odom_rot_rel_, diagnostics_imu_rot_rel_; // tf transformer - tf::Transformer transformer_; - + std::shared_ptr tf_buffer_; + std::shared_ptr tf_listener_; + + // tf2_ros::Buffer tf_buffer_; + // tf2_ros::TransformListener tf_listener_; + std::string output_frame_; std::string base_footprint_frame_; + + std::string hidden_output_frame_ = "hidden_output"; + std::string hidden_odom_frame_ = "hidden_odom"; + std::string hidden_imu_frame_ = "hidden_imu"; + std::string hidden_vo_frame_ = "hidden_vo"; + std::string hidden_gps_frame_ = "hidden_gps"; + + + }; // class }; // namespace diff --git a/include/robot_pose_ekf/odom_estimation_node.h b/include/robot_pose_ekf/odom_estimation_node.h index 5a33261..e3d40b1 100644 --- a/include/robot_pose_ekf/odom_estimation_node.h +++ b/include/robot_pose_ekf/odom_estimation_node.h @@ -38,9 +38,11 @@ // ros stuff #include -#include -#include -#include + +#include +#include +#include + #include "odom_estimation.h" #include @@ -55,6 +57,7 @@ // log files #include +#include namespace estimation { @@ -110,20 +113,25 @@ class OdomEstimationNode ros::ServiceServer state_srv_; // ekf filter - OdomEstimation my_filter_; + std::shared_ptr my_filter_; // estimated robot pose message to send geometry_msgs::PoseWithCovarianceStamped output_; // robot state - tf::TransformListener robot_state_; - tf::TransformBroadcaster odom_broadcaster_; + // tf transformer + std::shared_ptr tf_buffer_; + std::shared_ptr tf_listener_; + + + // tf::TransformListener robot_state_; + // tf::TransformBroadcaster odom_broadcaster_; // vectors - tf::Transform odom_meas_, imu_meas_, vo_meas_, gps_meas_; - tf::Transform base_vo_init_; - tf::Transform base_gps_init_; - tf::StampedTransform camera_base_; + tf2::Transform odom_meas_, imu_meas_, vo_meas_, gps_meas_; + tf2::Transform base_vo_init_; + tf2::Transform base_gps_init_; + geometry_msgs::TransformStamped camera_base_; ros::Time odom_time_, imu_time_, vo_time_, gps_time_; ros::Time odom_stamp_, imu_stamp_, vo_stamp_, gps_stamp_, filter_stamp_; ros::Time odom_init_stamp_, imu_init_stamp_, vo_init_stamp_, gps_init_stamp_; diff --git a/package.xml b/package.xml index 48c7194..c957685 100644 --- a/package.xml +++ b/package.xml @@ -21,7 +21,9 @@ geometry_msgs sensor_msgs nav_msgs - tf + tf2 + tf2_ros + tf2_geometry_msgs message_generation message_runtime @@ -32,7 +34,9 @@ geometry_msgs sensor_msgs nav_msgs - tf + tf2 + tf2_ros + tf2_geometry_msgs rosbag rostest diff --git a/src/odom_estimation.cpp b/src/odom_estimation.cpp index 388095e..94692e3 100644 --- a/src/odom_estimation.cpp +++ b/src/odom_estimation.cpp @@ -38,7 +38,7 @@ using namespace MatrixWrapper; using namespace BFL; -using namespace tf; +using namespace tf2; using namespace std; using namespace ros; @@ -46,7 +46,9 @@ using namespace ros; namespace estimation { // constructor - OdomEstimation::OdomEstimation(): + OdomEstimation::OdomEstimation( + std::shared_ptr tf_buffer, + std::shared_ptr tf_listener): prior_(NULL), filter_(NULL), filter_initialized_(false), @@ -55,12 +57,17 @@ namespace estimation vo_initialized_(false), gps_initialized_(false), output_frame_(std::string("odom_combined")), - base_footprint_frame_(std::string("base_footprint")) + base_footprint_frame_(std::string("base_footprint")), + tf_buffer_(tf_buffer), + tf_listener_(tf_listener) { // create SYSTEM MODEL ColumnVector sysNoise_Mu(6); sysNoise_Mu = 0; SymmetricMatrix sysNoise_Cov(6); sysNoise_Cov = 0; - for (unsigned int i=1; i<=6; i++) sysNoise_Cov(i,i) = pow(1000.0,2); + for (unsigned int i=1; i<=6; i++) + { + sysNoise_Cov(i,i) = pow(1000.0,2); + } Gaussian system_Uncertainty(sysNoise_Mu, sysNoise_Cov); sys_pdf_ = new NonLinearAnalyticConditionalGaussianOdo(system_Uncertainty); sys_model_ = new AnalyticSystemModelGaussianUncertainty(sys_pdf_); @@ -68,7 +75,10 @@ namespace estimation // create MEASUREMENT MODEL ODOM ColumnVector measNoiseOdom_Mu(6); measNoiseOdom_Mu = 0; SymmetricMatrix measNoiseOdom_Cov(6); measNoiseOdom_Cov = 0; - for (unsigned int i=1; i<=6; i++) measNoiseOdom_Cov(i,i) = 1; + for (unsigned int i=1; i<=6; i++) + { + measNoiseOdom_Cov(i,i) = 1; + } Gaussian measurement_Uncertainty_Odom(measNoiseOdom_Mu, measNoiseOdom_Cov); Matrix Hodom(6,6); Hodom = 0; Hodom(1,1) = 1; Hodom(2,2) = 1; Hodom(6,6) = 1; @@ -78,7 +88,10 @@ namespace estimation // create MEASUREMENT MODEL IMU ColumnVector measNoiseImu_Mu(3); measNoiseImu_Mu = 0; SymmetricMatrix measNoiseImu_Cov(3); measNoiseImu_Cov = 0; - for (unsigned int i=1; i<=3; i++) measNoiseImu_Cov(i,i) = 1; + for (unsigned int i=1; i<=3; i++) + { + measNoiseImu_Cov(i,i) = 1; + } Gaussian measurement_Uncertainty_Imu(measNoiseImu_Mu, measNoiseImu_Cov); Matrix Himu(3,6); Himu = 0; Himu(1,4) = 1; Himu(2,5) = 1; Himu(3,6) = 1; @@ -98,13 +111,16 @@ namespace estimation // create MEASUREMENT MODEL GPS ColumnVector measNoiseGps_Mu(3); measNoiseGps_Mu = 0; SymmetricMatrix measNoiseGps_Cov(3); measNoiseGps_Cov = 0; - for (unsigned int i=1; i<=3; i++) measNoiseGps_Cov(i,i) = 1; + for (unsigned int i=1; i<=3; i++) + { + measNoiseGps_Cov(i,i) = 1; + } Gaussian measurement_Uncertainty_GPS(measNoiseGps_Mu, measNoiseGps_Cov); Matrix Hgps(3,6); Hgps = 0; Hgps(1,1) = 1; Hgps(2,2) = 1; Hgps(3,3) = 1; gps_meas_pdf_ = new LinearAnalyticConditionalGaussian(Hgps, measurement_Uncertainty_GPS); gps_meas_model_ = new LinearAnalyticMeasurementModelGaussianUncertainty(gps_meas_pdf_); - }; + } @@ -122,7 +138,7 @@ namespace estimation delete gps_meas_pdf_; delete sys_pdf_; delete sys_model_; - }; + } // initialize prior density of filter @@ -132,17 +148,32 @@ namespace estimation ColumnVector prior_Mu(6); decomposeTransform(prior, prior_Mu(1), prior_Mu(2), prior_Mu(3), prior_Mu(4), prior_Mu(5), prior_Mu(6)); SymmetricMatrix prior_Cov(6); - for (unsigned int i=1; i<=6; i++) { - for (unsigned int j=1; j<=6; j++){ - if (i==j) prior_Cov(i,j) = pow(0.001,2); - else prior_Cov(i,j) = 0; + for (unsigned int i=1; i<=6; i++) + { + for (unsigned int j=1; j<=6; j++) + { + if (i==j) + { + prior_Cov(i,j) = pow(0.001,2); + } + else + { + prior_Cov(i,j) = 0; + } } } prior_ = new Gaussian(prior_Mu,prior_Cov); filter_ = new ExtendedKalmanFilter(prior_); // remember prior - addMeasurement(StampedTransform(prior, time, output_frame_, base_footprint_frame_)); + geometry_msgs::TransformStamped T; + + T.child_frame_id = hidden_output_frame_; + T.header.frame_id = base_footprint_frame_; + T.header.stamp = time; + tf2::convert(prior.inverse(), T.transform); + + addMeasurement(T); filter_estimate_old_vec_ = prior_Mu; filter_estimate_old_ = prior; filter_time_old_ = time; @@ -151,23 +182,26 @@ namespace estimation filter_initialized_ = true; } - - - - // update filter - bool OdomEstimation::update(bool odom_active, bool imu_active, bool gps_active, bool vo_active, const Time& filter_time, bool& diagnostics_res) + bool OdomEstimation::update( + bool odom_active, bool imu_active, bool gps_active, bool vo_active, + const Time& filter_time, bool& diagnostics_res) { // only update filter when it is initialized - if (!filter_initialized_){ + if (!filter_initialized_) + { ROS_INFO("Cannot update filter when filter was not initialized first."); return false; } // only update filter for time later than current filter time double dt = (filter_time - filter_time_old_).toSec(); - if (dt == 0) return false; - if (dt < 0){ + if (dt == 0) + { + return false; + } + if (dt < 0) + { ROS_INFO("Will not update robot pose with time %f sec in the past.", dt); return false; } @@ -184,103 +218,167 @@ namespace estimation // process odom measurement // ------------------------ ROS_DEBUG("Process odom meas"); - if (odom_active){ - if (!transformer_.canTransform(base_footprint_frame_,"wheelodom", filter_time)){ + if (odom_active) + { + if (!tf_buffer_->canTransform(base_footprint_frame_, hidden_odom_frame_, filter_time)) + { ROS_ERROR("filter time older than odom message buffer"); return false; } - transformer_.lookupTransform("wheelodom", base_footprint_frame_, filter_time, odom_meas_); - if (odom_initialized_){ - // convert absolute odom measurements to relative odom measurements in horizontal plane - Transform odom_rel_frame = Transform(tf::createQuaternionFromYaw(filter_estimate_old_vec_(6)), - filter_estimate_old_.getOrigin()) * odom_meas_old_.inverse() * odom_meas_; - ColumnVector odom_rel(6); - decomposeTransform(odom_rel_frame, odom_rel(1), odom_rel(2), odom_rel(3), odom_rel(4), odom_rel(5), odom_rel(6)); - angleOverflowCorrect(odom_rel(6), filter_estimate_old_vec_(6)); - // update filter - odom_meas_pdf_->AdditiveNoiseSigmaSet(odom_covariance_ * pow(dt,2)); + try { + odom_meas_ = tf_buffer_->lookupTransform(hidden_odom_frame_, base_footprint_frame_, filter_time); + } catch(tf2::TransformException &ex) { + std::cout << "ERROR (lookupTransform) - OdomEstimation::update() - wheelodom" << std::endl; + } + + if (odom_initialized_) + { + // convert absolute odom measurements to relative odom measurements in horizontal plane + Quaternion q; + Transform old_tf2, current_tf2; + { + q.setRPY(0, 0, filter_estimate_old_vec_(6)); + tf2::convert(odom_meas_old_.transform, old_tf2); + tf2::convert(odom_meas_.transform, current_tf2); + } + Transform odom_rel_frame = Transform(q, filter_estimate_old_.getOrigin()) * old_tf2.inverse() * current_tf2; + + ColumnVector odom_rel(6); + decomposeTransform(odom_rel_frame, odom_rel(1), odom_rel(2), odom_rel(3), odom_rel(4), odom_rel(5), odom_rel(6)); + angleOverflowCorrect(odom_rel(6), filter_estimate_old_vec_(6)); + // update filter + odom_meas_pdf_->AdditiveNoiseSigmaSet(odom_covariance_ * pow(dt,2)); ROS_DEBUG("Update filter with odom measurement %f %f %f %f %f %f", odom_rel(1), odom_rel(2), odom_rel(3), odom_rel(4), odom_rel(5), odom_rel(6)); - filter_->Update(odom_meas_model_, odom_rel); - diagnostics_odom_rot_rel_ = odom_rel(6); + filter_->Update(odom_meas_model_, odom_rel); + diagnostics_odom_rot_rel_ = odom_rel(6); } - else{ - odom_initialized_ = true; - diagnostics_odom_rot_rel_ = 0; + else + { + odom_initialized_ = true; + diagnostics_odom_rot_rel_ = 0; } odom_meas_old_ = odom_meas_; } - // sensor not active - else odom_initialized_ = false; + else // sensor not active + { + odom_initialized_ = false; + } - // process imu measurement // ----------------------- - if (imu_active){ - if (!transformer_.canTransform(base_footprint_frame_,"imu", filter_time)){ + if (imu_active) + { + if (!tf_buffer_->canTransform(base_footprint_frame_, hidden_imu_frame_, filter_time)) + { ROS_ERROR("filter time older than imu message buffer"); return false; } - transformer_.lookupTransform("imu", base_footprint_frame_, filter_time, imu_meas_); - if (imu_initialized_){ - // convert absolute imu yaw measurement to relative imu yaw measurement - Transform imu_rel_frame = filter_estimate_old_ * imu_meas_old_.inverse() * imu_meas_; - ColumnVector imu_rel(3); double tmp; - decomposeTransform(imu_rel_frame, tmp, tmp, tmp, tmp, tmp, imu_rel(3)); - decomposeTransform(imu_meas_, tmp, tmp, tmp, imu_rel(1), imu_rel(2), tmp); - angleOverflowCorrect(imu_rel(3), filter_estimate_old_vec_(6)); - diagnostics_imu_rot_rel_ = imu_rel(3); - // update filter - imu_meas_pdf_->AdditiveNoiseSigmaSet(imu_covariance_ * pow(dt,2)); - filter_->Update(imu_meas_model_, imu_rel); + try { + imu_meas_ = tf_buffer_->lookupTransform(hidden_imu_frame_, base_footprint_frame_, filter_time); + } catch(tf2::TransformException &ex) { + std::cout << "ERROR (lookupTransform) - OdomEstimation::update() - imu" << std::endl; + } + + if (imu_initialized_) + { + // convert absolute imu yaw measurement to relative imu yaw measurement + + Transform old_tf2, current_tf2; + { + tf2::convert(imu_meas_old_.transform, old_tf2); + tf2::convert(imu_meas_.transform, current_tf2); + } + + Transform imu_rel_frame = filter_estimate_old_ * old_tf2.inverse() * current_tf2; + ColumnVector imu_rel(3); double tmp; + decomposeTransform(imu_rel_frame, tmp, tmp, tmp, tmp, tmp, imu_rel(3)); + decomposeTransform(imu_meas_, tmp, tmp, tmp, imu_rel(1), imu_rel(2), tmp); + angleOverflowCorrect(imu_rel(3), filter_estimate_old_vec_(6)); + diagnostics_imu_rot_rel_ = imu_rel(3); + // update filter + imu_meas_pdf_->AdditiveNoiseSigmaSet(imu_covariance_ * pow(dt,2)); + filter_->Update(imu_meas_model_, imu_rel); } - else{ - imu_initialized_ = true; - diagnostics_imu_rot_rel_ = 0; + else + { + imu_initialized_ = true; + diagnostics_imu_rot_rel_ = 0; } imu_meas_old_ = imu_meas_; } - // sensor not active - else imu_initialized_ = false; + else // sensor not active + { + imu_initialized_ = false; + } // process vo measurement // ---------------------- - if (vo_active){ - if (!transformer_.canTransform(base_footprint_frame_,"vo", filter_time)){ + if (vo_active) + { + if (!tf_buffer_->canTransform(base_footprint_frame_, hidden_vo_frame_, filter_time)) + { ROS_ERROR("filter time older than vo message buffer"); return false; } - transformer_.lookupTransform("vo", base_footprint_frame_, filter_time, vo_meas_); - if (vo_initialized_){ - // convert absolute vo measurements to relative vo measurements - Transform vo_rel_frame = filter_estimate_old_ * vo_meas_old_.inverse() * vo_meas_; - ColumnVector vo_rel(6); - decomposeTransform(vo_rel_frame, vo_rel(1), vo_rel(2), vo_rel(3), vo_rel(4), vo_rel(5), vo_rel(6)); - angleOverflowCorrect(vo_rel(6), filter_estimate_old_vec_(6)); - // update filter + try { + vo_meas_ = tf_buffer_->lookupTransform(hidden_vo_frame_, base_footprint_frame_, filter_time); + } catch(tf2::TransformException &ex) { + std::cout << "ERROR (lookupTransform) - OdomEstimation::update() - vo" << std::endl; + } + + if (vo_initialized_) + { + // convert absolute vo measurements to relative vo measurements + + Transform old_tf2, current_tf2; + { + tf2::convert(vo_meas_old_.transform, old_tf2); + tf2::convert(vo_meas_.transform, current_tf2); + } + + Transform vo_rel_frame = filter_estimate_old_ * old_tf2.inverse() * current_tf2; + ColumnVector vo_rel(6); + decomposeTransform(vo_rel_frame, vo_rel(1), vo_rel(2), vo_rel(3), vo_rel(4), vo_rel(5), vo_rel(6)); + angleOverflowCorrect(vo_rel(6), filter_estimate_old_vec_(6)); + // update filter vo_meas_pdf_->AdditiveNoiseSigmaSet(vo_covariance_ * pow(dt,2)); filter_->Update(vo_meas_model_, vo_rel); } - else vo_initialized_ = true; + else + { + vo_initialized_ = true; + } vo_meas_old_ = vo_meas_; } - // sensor not active - else vo_initialized_ = false; + else // sensor not active + { + vo_initialized_ = false; + } // process gps measurement // ---------------------- - if (gps_active){ - if (!transformer_.canTransform(base_footprint_frame_,"gps", filter_time)){ + if (gps_active) + { + if (!tf_buffer_->canTransform(base_footprint_frame_, hidden_gps_frame_, filter_time)) + { ROS_ERROR("filter time older than gps message buffer"); return false; } - transformer_.lookupTransform("gps", base_footprint_frame_, filter_time, gps_meas_); - if (gps_initialized_){ + try + { + gps_meas_ = tf_buffer_->lookupTransform(hidden_gps_frame_, base_footprint_frame_, filter_time); + } catch(tf2::TransformException &ex) { + std::cout << "ERROR (lookupTransform) - OdomEstimation::update() - gps" << std::endl; + } + + if (gps_initialized_) + { gps_meas_pdf_->AdditiveNoiseSigmaSet(gps_covariance_ * pow(dt,2)); ColumnVector gps_vec(3); double tmp; @@ -288,148 +386,202 @@ namespace estimation decomposeTransform(gps_meas_, gps_vec(1), gps_vec(2), gps_vec(3), tmp, tmp, tmp); filter_->Update(gps_meas_model_, gps_vec); } - else { + else + { gps_initialized_ = true; gps_meas_old_ = gps_meas_; } } - // sensor not active - else gps_initialized_ = false; + else // sensor not active + { + gps_initialized_ = false; + } // remember last estimate filter_estimate_old_vec_ = filter_->PostGet()->ExpectedValueGet(); - tf::Quaternion q; + tf2::Quaternion q; q.setRPY(filter_estimate_old_vec_(4), filter_estimate_old_vec_(5), filter_estimate_old_vec_(6)); filter_estimate_old_ = Transform(q, Vector3(filter_estimate_old_vec_(1), filter_estimate_old_vec_(2), filter_estimate_old_vec_(3))); filter_time_old_ = filter_time; - addMeasurement(StampedTransform(filter_estimate_old_, filter_time, output_frame_, base_footprint_frame_)); + + + geometry_msgs::TransformStamped T; + + T.child_frame_id = hidden_output_frame_; + T.header.frame_id = base_footprint_frame_; + T.header.stamp = filter_time; + tf2::convert(filter_estimate_old_.inverse(), T.transform); + + + addMeasurement(T); // diagnostics diagnostics_res = true; - if (odom_active && imu_active){ + if (odom_active && imu_active) + { double diagnostics = fabs(diagnostics_odom_rot_rel_ - diagnostics_imu_rot_rel_)/dt; - if (diagnostics > 0.3 && dt > 0.01){ - diagnostics_res = false; + if (diagnostics > 0.3 && dt > 0.01) + { + diagnostics_res = false; } } return true; - }; + } - void OdomEstimation::addMeasurement(const StampedTransform& meas) + void OdomEstimation::addMeasurement( + const geometry_msgs::TransformStamped& meas) { - ROS_DEBUG("AddMeasurement from %s to %s: (%f, %f, %f) (%f, %f, %f, %f)", - meas.frame_id_.c_str(), meas.child_frame_id_.c_str(), - meas.getOrigin().x(), meas.getOrigin().y(), meas.getOrigin().z(), - meas.getRotation().x(), meas.getRotation().y(), - meas.getRotation().z(), meas.getRotation().w()); - transformer_.setTransform( meas ); + ROS_DEBUG_STREAM("AddMeasurement from " << meas.header.frame_id << " to " << meas.child_frame_id << " (T=" << meas.header.stamp << "): " + << " [" << meas.transform.translation.x << ", " << meas.transform.translation.y << ", " << meas.transform.translation.z << "]v" + << " [" << meas.transform.rotation.x << ", " << meas.transform.rotation.y << ", " << meas.transform.rotation.z << ", " << meas.transform.rotation.w << "]q"); + + tf_buffer_->setTransform( meas, "default_authority" ); } - void OdomEstimation::addMeasurement(const StampedTransform& meas, const MatrixWrapper::SymmetricMatrix& covar) + void OdomEstimation::addMeasurement( + const geometry_msgs::TransformStamped& meas, const MatrixWrapper::SymmetricMatrix& covar) { // check covariance - for (unsigned int i=0; icanTransform(base_footprint_frame_, hidden_output_frame_, time)) + { ROS_ERROR("Cannot get transform at time %f", time.toSec()); return; } - transformer_.lookupTransform(output_frame_, base_footprint_frame_, time, tmp); - estimate = tmp; - }; + try { + tmp = tf_buffer_->lookupTransform(hidden_output_frame_, base_footprint_frame_, time); + } catch(tf2::TransformException &ex) { + std::cout << "ERROR (lookupTransform) - OdomEstimation::getEstimate(Time time, Transform& estimate)" << std::endl; + } + tf2::convert(tmp.transform, estimate); + } // get filter posterior at time 'time' as Stamped Transform - void OdomEstimation::getEstimate(Time time, StampedTransform& estimate) + void OdomEstimation::getEstimate(Time time, geometry_msgs::TransformStamped& estimate) { - if (!transformer_.canTransform(output_frame_, base_footprint_frame_, time)){ + if (!tf_buffer_->canTransform(hidden_output_frame_, base_footprint_frame_, time)) + { ROS_ERROR("Cannot get transform at time %f", time.toSec()); return; } - transformer_.lookupTransform(output_frame_, base_footprint_frame_, time, estimate); - }; + try { + estimate = tf_buffer_->lookupTransform(hidden_output_frame_, base_footprint_frame_, time); + } catch(tf2::TransformException &ex) { + std::cout << "ERROR (lookupTransform) - OdomEstimation::getEstimate(Time time, geometry_msgs::TransformStamped& estimate)" << std::endl; + } + + // replace hidden output frame with real + estimate.header.frame_id = output_frame_; + } // get most recent filter posterior as PoseWithCovarianceStamped void OdomEstimation::getEstimate(geometry_msgs::PoseWithCovarianceStamped& estimate) { // pose - StampedTransform tmp; - if (!transformer_.canTransform(output_frame_, base_footprint_frame_, ros::Time())){ + geometry_msgs::TransformStamped tmp; + if (!tf_buffer_->canTransform(hidden_output_frame_, base_footprint_frame_, ros::Time())) + { ROS_ERROR("Cannot get transform at time %f", 0.0); return; } - transformer_.lookupTransform(output_frame_, base_footprint_frame_, ros::Time(), tmp); - poseTFToMsg(tmp, estimate.pose.pose); + try { + tmp = tf_buffer_->lookupTransform(hidden_output_frame_, base_footprint_frame_, ros::Time()); + } catch(tf2::TransformException &ex) { + std::cout << "ERROR (lookupTransform) - OdomEstimation::getEstimate(geometry_msgs::PoseWithCovarianceStamped& estimate)" << std::endl; + } + + estimate.pose.pose.position.x = tmp.transform.translation.x; + estimate.pose.pose.position.y = tmp.transform.translation.y; + estimate.pose.pose.position.z = tmp.transform.translation.z; + estimate.pose.pose.orientation.x = tmp.transform.rotation.x; + estimate.pose.pose.orientation.y = tmp.transform.rotation.y; + estimate.pose.pose.orientation.z = tmp.transform.rotation.z; + estimate.pose.pose.orientation.w = tmp.transform.rotation.w; // header - estimate.header.stamp = tmp.stamp_; - estimate.header.frame_id = output_frame_; + estimate.header.stamp = tmp.header.stamp; + estimate.header.frame_id = hidden_output_frame_; // covariance SymmetricMatrix covar = filter_->PostGet()->CovarianceGet(); for (unsigned int i=0; i<6; i++) + { for (unsigned int j=0; j<6; j++) - estimate.pose.covariance[6*i+j] = covar(i+1,j+1); - }; + { + estimate.pose.covariance[6*i+j] = covar(i+1,j+1); + } + } + } // correct for angle overflow void OdomEstimation::angleOverflowCorrect(double& a, double ref) { + // TODO: better use atan? while ((a-ref) > M_PI) a -= 2*M_PI; while ((a-ref) < -M_PI) a += 2*M_PI; - }; + } // decompose Transform into x,y,z,Rx,Ry,Rz - void OdomEstimation::decomposeTransform(const StampedTransform& trans, - double& x, double& y, double&z, double&Rx, double& Ry, double& Rz){ - x = trans.getOrigin().x(); - y = trans.getOrigin().y(); - z = trans.getOrigin().z(); - trans.getBasis().getEulerYPR(Rz, Ry, Rx); - }; + void OdomEstimation::decomposeTransform(const geometry_msgs::TransformStamped& trans, + double& x, double& y, double&z, double&Rx, double& Ry, double& Rz) + { + tf2::Transform T; + tf2::convert(trans.transform, T); + x = T.getOrigin().x(); + y = T.getOrigin().y(); + z = T.getOrigin().z(); + T.getBasis().getEulerYPR(Rz, Ry, Rx); + } // decompose Transform into x,y,z,Rx,Ry,Rz void OdomEstimation::decomposeTransform(const Transform& trans, - double& x, double& y, double&z, double&Rx, double& Ry, double& Rz){ + double& x, double& y, double&z, double&Rx, double& Ry, double& Rz) + { x = trans.getOrigin().x(); y = trans.getOrigin().y(); z = trans.getOrigin().z(); trans.getBasis().getEulerYPR(Rz, Ry, Rx); - }; + } - void OdomEstimation::setOutputFrame(const std::string& output_frame){ - output_frame_ = output_frame; - }; + void OdomEstimation::setBaseFootprintFrame(const std::string& base_frame) + { + base_footprint_frame_ = base_frame; + } - void OdomEstimation::setBaseFootprintFrame(const std::string& base_frame){ - base_footprint_frame_ = base_frame; - }; + void OdomEstimation::setOutputFrame(const std::string& output_frame) + { + output_frame_ = output_frame; + } }; // namespace diff --git a/src/odom_estimation_node.cpp b/src/odom_estimation_node.cpp index dc9eb90..8f5dd86 100644 --- a/src/odom_estimation_node.cpp +++ b/src/odom_estimation_node.cpp @@ -40,7 +40,7 @@ using namespace MatrixWrapper; using namespace std; using namespace ros; -using namespace tf; +using namespace tf2; static const double EPS = 1e-5; @@ -73,6 +73,12 @@ namespace estimation ros::NodeHandle nh_private("~"); ros::NodeHandle nh; + tf_buffer_ = std::make_shared(); + tf_listener_ = std::make_shared(*tf_buffer_); + + // TODO: pass tf_buffer here + my_filter_ = std::make_shared(tf_buffer_, tf_listener_); + // paramters nh_private.param("output_frame", output_frame_, std::string("odom_combined")); nh_private.param("base_footprint_frame", base_footprint_frame_, std::string("base_footprint")); @@ -86,17 +92,24 @@ namespace estimation double freq; nh_private.param("freq", freq, 30.0); - tf_prefix_ = tf::getPrefixParam(nh_private); - output_frame_ = tf::resolve(tf_prefix_, output_frame_); - base_footprint_frame_ = tf::resolve(tf_prefix_, base_footprint_frame_); + + // how to handle this? + // tf_prefix_ = tf::getPrefixParam(nh_private); + + // output_frame_ = tf::resolve(tf_prefix_, output_frame_); + // base_footprint_frame_ = tf::resolve(tf_prefix_, base_footprint_frame_); + ROS_INFO_STREAM("output frame: " << output_frame_); ROS_INFO_STREAM("base frame: " << base_footprint_frame_); + // set output frame and base frame names in OdomEstimation filter // so that user-defined tf frames are respected - my_filter_.setOutputFrame(output_frame_); - my_filter_.setBaseFootprintFrame(base_footprint_frame_); + my_filter_->setOutputFrame(output_frame_); + + + my_filter_->setBaseFootprintFrame(base_footprint_frame_); timer_ = nh_private.createTimer(ros::Duration(1.0/max(freq,1.0)), &OdomEstimationNode::spin, this); @@ -107,53 +120,67 @@ namespace estimation filter_stamp_ = Time::now(); // subscribe to odom messages - if (odom_used_){ + if (odom_used_) + { ROS_DEBUG("Odom sensor can be used"); odom_sub_ = nh.subscribe("odom", 10, &OdomEstimationNode::odomCallback, this); } - else ROS_DEBUG("Odom sensor will NOT be used"); + else + { + ROS_DEBUG("Odom sensor will NOT be used"); + } // subscribe to imu messages - if (imu_used_){ + if (imu_used_) + { ROS_DEBUG("Imu sensor can be used"); imu_sub_ = nh.subscribe("imu_data", 10, &OdomEstimationNode::imuCallback, this); } - else ROS_DEBUG("Imu sensor will NOT be used"); + else + { + ROS_DEBUG("Imu sensor will NOT be used"); + } // subscribe to vo messages - if (vo_used_){ + if (vo_used_) + { ROS_DEBUG("VO sensor can be used"); vo_sub_ = nh.subscribe("vo", 10, &OdomEstimationNode::voCallback, this); } - else ROS_DEBUG("VO sensor will NOT be used"); + else + { + ROS_DEBUG("VO sensor will NOT be used"); + } - if (gps_used_){ + if (gps_used_) + { ROS_DEBUG("GPS sensor can be used"); gps_sub_ = nh.subscribe("gps", 10, &OdomEstimationNode::gpsCallback, this); } - else ROS_DEBUG("GPS sensor will NOT be used"); + else + { + ROS_DEBUG("GPS sensor will NOT be used"); + } // publish state service state_srv_ = nh_private.advertiseService("get_status", &OdomEstimationNode::getStatus, this); - if (debug_){ + if (debug_) + { // open files for debugging odom_file_.open("/tmp/odom_file.txt"); imu_file_.open("/tmp/imu_file.txt"); vo_file_.open("/tmp/vo_file.txt"); gps_file_.open("/tmp/gps_file.txt"); corr_file_.open("/tmp/corr_file.txt"); - - } }; - - // destructor - OdomEstimationNode::~OdomEstimationNode(){ + OdomEstimationNode::~OdomEstimationNode() + { if (debug_){ // close files for debugging @@ -165,10 +192,6 @@ namespace estimation } }; - - - - // callback function for odom data void OdomEstimationNode::odomCallback(const OdomConstPtr& odom) { @@ -181,31 +204,48 @@ namespace estimation odom_stamp_ = odom->header.stamp; odom_time_ = Time::now(); Quaternion q; - tf::quaternionMsgToTF(odom->pose.pose.orientation, q); + tf2::convert(odom->pose.pose.orientation, q); odom_meas_ = Transform(q, Vector3(odom->pose.pose.position.x, odom->pose.pose.position.y, 0)); for (unsigned int i=0; i<6; i++) + { for (unsigned int j=0; j<6; j++) + { odom_covariance_(i+1, j+1) = odom->pose.covariance[6*i+j]; + } + } + + geometry_msgs::TransformStamped odom_meas_inv; + tf2::convert(odom_meas_.inverse(), odom_meas_inv.transform); + odom_meas_inv.header.stamp = odom_stamp_; + odom_meas_inv.header.frame_id = base_footprint_frame_; + odom_meas_inv.child_frame_id = "hidden_odom"; - my_filter_.addMeasurement(StampedTransform(odom_meas_.inverse(), odom_stamp_, base_footprint_frame_, "wheelodom"), odom_covariance_); + my_filter_->addMeasurement(odom_meas_inv, odom_covariance_); // activate odom - if (!odom_active_) { - if (!odom_initializing_){ - odom_initializing_ = true; - odom_init_stamp_ = odom_stamp_; - ROS_INFO("Initializing Odom sensor"); + if (!odom_active_) + { + if (!odom_initializing_) + { + odom_initializing_ = true; + odom_init_stamp_ = odom_stamp_; + ROS_INFO("Initializing Odom sensor"); } - if ( filter_stamp_ >= odom_init_stamp_){ - odom_active_ = true; - odom_initializing_ = false; - ROS_INFO("Odom sensor activated"); + if ( filter_stamp_ >= odom_init_stamp_) + { + odom_active_ = true; + odom_initializing_ = false; + ROS_INFO("Odom sensor activated"); } - else ROS_DEBUG("Waiting to activate Odom, because Odom measurements are still %f sec in the future.", + else + { + ROS_DEBUG("Waiting to activate Odom, because Odom measurements are still %f sec in the future.", (odom_init_stamp_ - filter_stamp_).toSec()); + } } - if (debug_){ + if (debug_) + { // write to file double tmp, yaw; odom_meas_.getBasis().getEulerYPR(yaw, tmp, tmp); @@ -225,58 +265,88 @@ namespace estimation // receive data imu_stamp_ = imu->header.stamp; - tf::Quaternion orientation; - quaternionMsgToTF(imu->orientation, orientation); - imu_meas_ = tf::Transform(orientation, tf::Vector3(0,0,0)); + Quaternion orientation; + tf2::convert(imu->orientation, orientation); + imu_meas_ = Transform(orientation, Vector3(0,0,0)); for (unsigned int i=0; i<3; i++) + { for (unsigned int j=0; j<3; j++) + { imu_covariance_(i+1, j+1) = imu->orientation_covariance[3*i+j]; + } + } // Transforms imu data to base_footprint frame - if (!robot_state_.waitForTransform(base_footprint_frame_, imu->header.frame_id, imu_stamp_, ros::Duration(0.5))){ + if (!tf_buffer_->canTransform(base_footprint_frame_, imu->header.frame_id, imu_stamp_, ros::Duration(0.5))) + { // warn when imu was already activated, not when imu is not active yet if (imu_active_) ROS_ERROR("Could not transform imu message from %s to %s", imu->header.frame_id.c_str(), base_footprint_frame_.c_str()); - else if (my_filter_.isInitialized()) + else if (my_filter_->isInitialized()) ROS_WARN("Could not transform imu message from %s to %s. Imu will not be activated yet.", imu->header.frame_id.c_str(), base_footprint_frame_.c_str()); else ROS_DEBUG("Could not transform imu message from %s to %s. Imu will not be activated yet.", imu->header.frame_id.c_str(), base_footprint_frame_.c_str()); return; } - StampedTransform base_imu_offset; - robot_state_.lookupTransform(base_footprint_frame_, imu->header.frame_id, imu_stamp_, base_imu_offset); - imu_meas_ = imu_meas_ * base_imu_offset; + + geometry_msgs::TransformStamped base_imu_offset; + + try { + base_imu_offset = tf_buffer_->lookupTransform(base_footprint_frame_, imu->header.frame_id, imu_stamp_); + } catch(tf2::TransformException& ex) { + // TODO: what to do here? + std::cout << "ERROR (lookupTransform) - OdomEstimationNode::imuCallback()" << std::endl; + } + + Transform base_imu_offset_tf; + tf2::convert(base_imu_offset.transform, base_imu_offset_tf); + + imu_meas_ = imu_meas_ * base_imu_offset_tf; imu_time_ = Time::now(); // manually set covariance untile imu sends covariance - if (imu_covariance_(1,1) == 0.0){ - SymmetricMatrix measNoiseImu_Cov(3); measNoiseImu_Cov = 0; + if (imu_covariance_(1,1) == 0.0) + { + SymmetricMatrix measNoiseImu_Cov(3); + measNoiseImu_Cov = 0; measNoiseImu_Cov(1,1) = pow(0.00017,2); // = 0.01 degrees / sec measNoiseImu_Cov(2,2) = pow(0.00017,2); // = 0.01 degrees / sec measNoiseImu_Cov(3,3) = pow(0.00017,2); // = 0.01 degrees / sec imu_covariance_ = measNoiseImu_Cov; } - my_filter_.addMeasurement(StampedTransform(imu_meas_.inverse(), imu_stamp_, base_footprint_frame_, "imu"), imu_covariance_); + geometry_msgs::TransformStamped imu_meas_inv; + tf2::convert(imu_meas_.inverse(), imu_meas_inv.transform); + imu_meas_inv.header.stamp = imu_stamp_; + imu_meas_inv.header.frame_id = base_footprint_frame_; + imu_meas_inv.child_frame_id = "hidden_imu"; + my_filter_->addMeasurement(imu_meas_inv, imu_covariance_); // activate imu - if (!imu_active_) { - if (!imu_initializing_){ - imu_initializing_ = true; - imu_init_stamp_ = imu_stamp_; - ROS_INFO("Initializing Imu sensor"); + if (!imu_active_) + { + if (!imu_initializing_) + { + imu_initializing_ = true; + imu_init_stamp_ = imu_stamp_; + ROS_INFO("Initializing Imu sensor"); } - if ( filter_stamp_ >= imu_init_stamp_){ - imu_active_ = true; - imu_initializing_ = false; - ROS_INFO("Imu sensor activated"); + if ( filter_stamp_ >= imu_init_stamp_) + { + imu_active_ = true; + imu_initializing_ = false; + ROS_INFO("Imu sensor activated"); } - else ROS_DEBUG("Waiting to activate IMU, because IMU measurements are still %f sec in the future.", + else + { + ROS_DEBUG("Waiting to activate IMU, because IMU measurements are still %f sec in the future.", (imu_init_stamp_ - filter_stamp_).toSec()); + } } - if (debug_){ + if (debug_) + { // write to file double tmp, yaw; imu_meas_.getBasis().getEulerYPR(yaw, tmp, tmp); @@ -297,26 +367,42 @@ namespace estimation // get data vo_stamp_ = vo->header.stamp; vo_time_ = Time::now(); - poseMsgToTF(vo->pose.pose, vo_meas_); + // poseMsgToTF(vo->pose.pose, vo_meas_); + tf2::convert(vo->pose.pose, vo_meas_); for (unsigned int i=0; i<6; i++) + { for (unsigned int j=0; j<6; j++) + { vo_covariance_(i+1, j+1) = vo->pose.covariance[6*i+j]; - my_filter_.addMeasurement(StampedTransform(vo_meas_.inverse(), vo_stamp_, base_footprint_frame_, "vo"), vo_covariance_); + } + } + geometry_msgs::TransformStamped vo_meas_inv; + tf2::convert(vo_meas_.inverse(), vo_meas_inv.transform); + vo_meas_inv.header.stamp = vo_stamp_; + vo_meas_inv.header.frame_id = base_footprint_frame_; + vo_meas_inv.child_frame_id = "hidden_vo"; + my_filter_->addMeasurement(vo_meas_inv, vo_covariance_); // activate vo - if (!vo_active_) { - if (!vo_initializing_){ - vo_initializing_ = true; - vo_init_stamp_ = vo_stamp_; - ROS_INFO("Initializing Vo sensor"); + if (!vo_active_) + { + if (!vo_initializing_) + { + vo_initializing_ = true; + vo_init_stamp_ = vo_stamp_; + ROS_INFO("Initializing Vo sensor"); } - if (filter_stamp_ >= vo_init_stamp_){ - vo_active_ = true; - vo_initializing_ = false; - ROS_INFO("Vo sensor activated"); + if (filter_stamp_ >= vo_init_stamp_) + { + vo_active_ = true; + vo_initializing_ = false; + ROS_INFO("Vo sensor activated"); } - else ROS_DEBUG("Waiting to activate VO, because VO measurements are still %f sec in the future.", + else + { + ROS_DEBUG("Waiting to activate VO, because VO measurements are still %f sec in the future.", (vo_init_stamp_ - filter_stamp_).toSec()); + } } if (debug_){ @@ -346,26 +432,42 @@ namespace estimation // set the covariance for the linear z component very high so we just ignore it gps_pose.covariance[6*2 + 2] = std::numeric_limits::max(); } - poseMsgToTF(gps_pose.pose, gps_meas_); + // poseMsgToTF(gps_pose.pose, gps_meas_); + tf2::convert(gps_pose.pose, gps_meas_); for (unsigned int i=0; i<3; i++) + { for (unsigned int j=0; j<3; j++) + { gps_covariance_(i+1, j+1) = gps_pose.covariance[6*i+j]; - my_filter_.addMeasurement(StampedTransform(gps_meas_.inverse(), gps_stamp_, base_footprint_frame_, "gps"), gps_covariance_); + } + } + geometry_msgs::TransformStamped gps_meas_inv; + tf2::convert(gps_meas_.inverse(), gps_meas_inv.transform); + gps_meas_inv.header.stamp = gps_stamp_; + gps_meas_inv.header.frame_id = base_footprint_frame_; + gps_meas_inv.child_frame_id = "hidden_gps"; + my_filter_->addMeasurement(gps_meas_inv, gps_covariance_); // activate gps - if (!gps_active_) { - if (!gps_initializing_){ - gps_initializing_ = true; - gps_init_stamp_ = gps_stamp_; - ROS_INFO("Initializing GPS sensor"); + if (!gps_active_) + { + if (!gps_initializing_) + { + gps_initializing_ = true; + gps_init_stamp_ = gps_stamp_; + ROS_INFO("Initializing GPS sensor"); } - if (filter_stamp_ >= gps_init_stamp_){ - gps_active_ = true; - gps_initializing_ = false; - ROS_INFO("GPS sensor activated"); + if (filter_stamp_ >= gps_init_stamp_) + { + gps_active_ = true; + gps_initializing_ = false; + ROS_INFO("GPS sensor activated"); } - else ROS_DEBUG("Waiting to activate GPS, because GPS measurements are still %f sec in the future.", + else + { + ROS_DEBUG("Waiting to activate GPS, because GPS measurements are still %f sec in the future.", (gps_init_stamp_ - filter_stamp_).toSec()); + } } if (debug_){ @@ -392,30 +494,35 @@ namespace estimation // check which sensors are still active if ((odom_active_ || odom_initializing_) && - (Time::now() - odom_time_).toSec() > timeout_){ + (Time::now() - odom_time_).toSec() > timeout_) + { odom_active_ = false; odom_initializing_ = false; ROS_INFO("Odom sensor not active any more"); } if ((imu_active_ || imu_initializing_) && - (Time::now() - imu_time_).toSec() > timeout_){ + (Time::now() - imu_time_).toSec() > timeout_) + { imu_active_ = false; imu_initializing_ = false; ROS_INFO("Imu sensor not active any more"); } if ((vo_active_ || vo_initializing_) && - (Time::now() - vo_time_).toSec() > timeout_){ + (Time::now() - vo_time_).toSec() > timeout_) + { vo_active_ = false; vo_initializing_ = false; ROS_INFO("VO sensor not active any more"); } if ((gps_active_ || gps_initializing_) && - (Time::now() - gps_time_).toSec() > timeout_){ + (Time::now() - gps_time_).toSec() > timeout_) + { gps_active_ = false; gps_initializing_ = false; ROS_INFO("GPS sensor not active any more"); } // only update filter when one of the sensors is active - if (odom_active_ || imu_active_ || vo_active_ || gps_active_){ + if (odom_active_ || imu_active_ || vo_active_ || gps_active_) + { // update filter at time where all sensor measurements are available if (odom_active_) filter_stamp_ = min(filter_stamp_, odom_stamp_); @@ -425,30 +532,42 @@ namespace estimation // update filter - if ( my_filter_.isInitialized() ) { + if ( my_filter_->isInitialized() ) + { bool diagnostics = true; - if (my_filter_.update(odom_active_, imu_active_,gps_active_, vo_active_, filter_stamp_, diagnostics)){ - + if (my_filter_->update(odom_active_, imu_active_,gps_active_, vo_active_, filter_stamp_, diagnostics)) + { // output most recent estimate and relative covariance - my_filter_.getEstimate(output_); + my_filter_->getEstimate(output_); pose_pub_.publish(output_); ekf_sent_counter_++; // broadcast most recent estimate to TransformArray - StampedTransform tmp; - my_filter_.getEstimate(ros::Time(), tmp); + geometry_msgs::TransformStamped tmp; + my_filter_->getEstimate(ros::Time(), tmp); if(!vo_active_ && !gps_active_) - tmp.getOrigin().setZ(0.0); - odom_broadcaster_.sendTransform(StampedTransform(tmp, tmp.stamp_, output_frame_, base_footprint_frame_)); + { + tmp.transform.translation.z = 0.0; + } + + // ROS_INFO_STREAM("br: " << tmp); + + tmp.header.frame_id = output_frame_; + tmp.child_frame_id = base_footprint_frame_; + static tf2_ros::TransformBroadcaster br; + br.sendTransform(tmp); - if (debug_){ + if (debug_) + { // write to file ColumnVector estimate; - my_filter_.getEstimate(estimate); + my_filter_->getEstimate(estimate); corr_file_ << fixed << setprecision(5)<isInitialized()) + { + Quaternion q = imu_meas_.getRotation(); Vector3 p = gps_meas_.getOrigin(); Transform init_meas_ = Transform(q, p); - my_filter_.initialize(init_meas_, gps_stamp_); + my_filter_->initialize(init_meas_, gps_stamp_); ROS_INFO("Kalman filter initialized with gps and imu measurement"); } - else if ( odom_active_ && gps_active_ && !my_filter_.isInitialized()) { - Quaternion q = odom_meas_.getRotation(); + else if ( odom_active_ && gps_active_ && !my_filter_->isInitialized()) + { + Quaternion q = odom_meas_.getRotation(); Vector3 p = gps_meas_.getOrigin(); Transform init_meas_ = Transform(q, p); - my_filter_.initialize(init_meas_, gps_stamp_); + my_filter_->initialize(init_meas_, gps_stamp_); ROS_INFO("Kalman filter initialized with gps and odometry measurement"); } - else if ( vo_active_ && gps_active_ && !my_filter_.isInitialized()) { - Quaternion q = vo_meas_.getRotation(); + else if ( vo_active_ && gps_active_ && !my_filter_->isInitialized()) + { + Quaternion q = vo_meas_.getRotation(); Vector3 p = gps_meas_.getOrigin(); Transform init_meas_ = Transform(q, p); - my_filter_.initialize(init_meas_, gps_stamp_); + my_filter_->initialize(init_meas_, gps_stamp_); ROS_INFO("Kalman filter initialized with gps and visual odometry measurement"); } - else if ( odom_active_ && !gps_used_ && !my_filter_.isInitialized()){ - my_filter_.initialize(odom_meas_, odom_stamp_); + else if ( odom_active_ && !gps_used_ && !my_filter_->isInitialized()) + { + my_filter_->initialize(odom_meas_, odom_stamp_); ROS_INFO("Kalman filter initialized with odom measurement"); } - else if ( vo_active_ && !gps_used_ && !my_filter_.isInitialized()){ - my_filter_.initialize(vo_meas_, vo_stamp_); + else if ( vo_active_ && !gps_used_ && !my_filter_->isInitialized()) + { + my_filter_->initialize(vo_meas_, vo_stamp_); ROS_INFO("Kalman filter initialized with vo measurement"); } } @@ -517,7 +641,7 @@ bool OdomEstimationNode::getStatus(robot_pose_ekf::GetStatus::Request& req, robo ss << " - listens to topic " << gps_sub_.getTopic() << endl; ss << "Output:" << endl; ss << " * Robot pose ekf filter" << endl; - ss << " - is "; if (!my_filter_.isInitialized()) ss << "NOT "; ss << "active" << endl; + ss << " - is "; if (!my_filter_->isInitialized()) ss << "NOT "; ss << "active" << endl; ss << " - sent " << ekf_sent_counter_ << " messages" << endl; ss << " - pulishes on topics " << pose_pub_.getTopic() << " and /tf" << endl; resp.status = ss.str();