From 8b9f2acd3c1ccfab6b5bc578c3ee4d1e31bbb78b Mon Sep 17 00:00:00 2001 From: pascalau Date: Sun, 15 Jan 2023 11:05:15 +0100 Subject: [PATCH 1/9] Migrated from ROS1 to ROS2 --- CMakeLists.txt | 148 ++++++- README.md | 8 +- ...{vicon_estimator.h => vicon_estimator.hpp} | 245 +++++------ ...timator.h => vicon_odometry_estimator.hpp} | 49 +-- launch/asl_optitrack.launch | 6 +- launch/asl_vicon.launch | 8 +- ...{viconEstimator.msg => ViconEstimator.msg} | 6 +- package.xml | 38 +- src/library/vicon_estimator.cpp | 338 +++++++--------- src/ros_vrpn_client.cpp | 383 +++++++++--------- src/test/include/test_helper_library.h | 14 +- src/test/library/test_helper_library.cpp | 26 +- src/test/test_vicon_estimator.cpp | 270 ++++++------ src/vicon_estimation_node.cpp | 124 +++--- src/wrapper/vicon_odometry_estimator.cpp | 219 +++++----- 15 files changed, 1002 insertions(+), 880 deletions(-) rename include/{vicon_estimator.h => vicon_estimator.hpp} (56%) rename include/{vicon_odometry_estimator.h => vicon_odometry_estimator.hpp} (63%) rename msg/{viconEstimator.msg => ViconEstimator.msg} (88%) diff --git a/CMakeLists.txt b/CMakeLists.txt index 6366cd5..a899c08 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -1,32 +1,148 @@ -cmake_minimum_required(VERSION 2.8.0) -project(ros_vrpn_client) +cmake_minimum_required(VERSION 3.5) +project(ros_vrpn) -add_definitions(-std=c++11) +# Default to C99 +if(NOT CMAKE_C_STANDARD) + set(CMAKE_C_STANDARD 99) +endif() -find_package(catkin_simple REQUIRED) -catkin_simple(ALL_DEPS_REQUIRED) +# Default to C++14 +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 14) +endif() -find_package(Eigen REQUIRED) -include_directories(${Eigen_INCLUDE_DIRS}) +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() -cs_add_library(vicon_estimator + +# Find dependencies +find_package(rclcpp REQUIRED) +find_package(rclcpp_components REQUIRED) +find_package(tf2_ros REQUIRED) +find_package(tf2_geometry_msgs REQUIRED) +find_package(tf2_eigen REQUIRED) +find_package(geometry_msgs REQUIRED) +find_package(nav_msgs REQUIRED) +find_package(vrpn_vendor REQUIRED) +find_package(rosidl_default_generators REQUIRED) +find_package(tf2_eigen REQUIRED) +find_package(eigen3_cmake_module REQUIRED) +find_package(Eigen3 REQUIRED) + +# Generate msgs +rosidl_generate_interfaces(${PROJECT_NAME} + msg/ViconEstimator.msg + DEPENDENCIES std_msgs geometry_msgs + ADD_LINTER_TESTS +) + +rosidl_get_typesupport_target(cpp_typesupport_target + ${PROJECT_NAME} "rosidl_typesupport_cpp") + +# Compile library +add_library(vicon_estimator src/library/vicon_estimator.cpp src/wrapper/vicon_odometry_estimator.cpp ) -cs_add_executable(ros_vrpn_client +rclcpp_components_register_node(vicon_estimator PLUGIN "vicon_estimator::ViconOdometryEstimator" EXECUTABLE vicon_odometry_estimator) + + +target_include_directories(vicon_estimator + PUBLIC + $ + $) + +ament_target_dependencies(vicon_estimator + rclcpp + rclcpp_components + Eigen3 + tf2_ros + tf2_geometry_msgs + tf2_eigen +) + +target_link_libraries(vicon_estimator "${cpp_typesupport_target}") + + +ament_export_dependencies( + rosidl_default_runtime + eigen3_cmake_module + Eigen3 +) + +# Compile ros_vrpn_client +add_executable(ros_vrpn_client src/ros_vrpn_client.cpp ) -cs_add_executable(vicon_estimation_node - src/vicon_estimation_node.cpp +target_include_directories(ros_vrpn_client + PUBLIC + $ + $) + +target_link_libraries(ros_vrpn_client vicon_estimator) + +ament_target_dependencies(ros_vrpn_client + rclcpp + Eigen3 + tf2_ros + tf2_geometry_msgs + tf2_eigen + vrpn_vendor + nav_msgs + geometry_msgs +) + +# Compile vicon_estimation_node +add_executable(vicon_estimation_node + src/vicon_estimation_node.cpp +) + +target_include_directories(vicon_estimation_node + PUBLIC + $ + $) + +target_link_libraries(vicon_estimation_node vicon_estimator) + +ament_target_dependencies(vicon_estimation_node + rclcpp + Eigen3 + tf2_ros + tf2_geometry_msgs + tf2_eigen + vrpn_vendor + nav_msgs + geometry_msgs +) + +# TODO(): Add tests back +# add_subdirectory(src/test) + +# Install targets +install( + TARGETS vicon_estimator + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin) + +install(DIRECTORY + launch + DESTINATION share/${PROJECT_NAME} ) -add_subdirectory(src/test) +install(TARGETS + ros_vrpn_client vicon_estimation_node + DESTINATION lib/${PROJECT_NAME}) -target_link_libraries(ros_vrpn_client vicon_estimator ${catkin_LIBRARIES}) -target_link_libraries(vicon_estimation_node vicon_estimator ${catkin_LIBRARIES}) +# Testing +if(BUILD_TESTING) + # Test linting + find_package(ament_lint_auto REQUIRED) + ament_lint_auto_find_test_dependencies() -cs_install() -cs_export() +endif() +ament_package() \ No newline at end of file diff --git a/README.md b/README.md index 55cf723..1f8587d 100644 --- a/README.md +++ b/README.md @@ -1,18 +1,18 @@ -ros_vrpn_client +ros_vrpn =============== -Ros interface for http://www.cs.unc.edu/Research/vrpn/ +Ros2 interface for http://www.cs.unc.edu/Research/vrpn/ Dependencies ------------------- -vrpn_catkin package from: https://github.com/ethz-asl/vrpn_catkin +vrpn_vendor package from: https://github.com/ethz-asl/vrpn_catkin (checkout ros2 branch) Usage ----------------- You have to start a ROS node per tracked object and the ROS node name has to be the name of the trackable object. - rosrun ros_vrpn_client ros_vrpn_client _object_name:=object_name _vrpn_server_ip:=192.168.1.1 + ros2 run ros_vrpn ros_vrpn_client --ros-args -p object_name:=object_name -p vrpn_server_ip:=192.168.1.1 Or in a launch file: ```XML diff --git a/include/vicon_estimator.h b/include/vicon_estimator.hpp similarity index 56% rename from include/vicon_estimator.h rename to include/vicon_estimator.hpp index 0402741..35faa71 100644 --- a/include/vicon_estimator.h +++ b/include/vicon_estimator.hpp @@ -24,12 +24,19 @@ #include -namespace vicon_estimator { +namespace vicon_estimator +{ // Estimator Status enum -enum class EstimatorStatus { OK, OUTLIER, RESET }; +enum class EstimatorStatus +{ + OK, + OUTLIER, + RESET +}; -enum class OutlierRejectionMethod { +enum class OutlierRejectionMethod +{ MAHALANOBIS_DISTANCE, SUBSEQUENT_MEASUREMENTS, NONE @@ -39,20 +46,21 @@ enum class OutlierRejectionMethod { // values static const double kDefaultTranslationalKp = 1.0; static const double kDefaultTranslationalKv = 10.0; -static const Eigen::Vector3d kDefaultInitialPositionEstimate = - Eigen::Vector3d::Zero(); -static const Eigen::Vector3d kDefaultInitialVelocityEstimate = - Eigen::Vector3d::Zero(); +static const Eigen::Vector3d kDefaultInitialPositionEstimate = Eigen::Vector3d::Zero(); +static const Eigen::Vector3d kDefaultInitialVelocityEstimate = Eigen::Vector3d::Zero(); -class TranslationalEstimatorParameters { - public: +class TranslationalEstimatorParameters +{ +public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW // Constructor TranslationalEstimatorParameters() - : kp_(kDefaultTranslationalKp), - kv_(kDefaultTranslationalKv), - initial_position_estimate_(kDefaultInitialPositionEstimate), - initial_velocity_estimate_(kDefaultInitialVelocityEstimate) {} + : kp_(kDefaultTranslationalKp), + kv_(kDefaultTranslationalKv), + initial_position_estimate_(kDefaultInitialPositionEstimate), + initial_velocity_estimate_(kDefaultInitialVelocityEstimate) + { + } double kp_; double kv_; @@ -60,16 +68,19 @@ class TranslationalEstimatorParameters { Eigen::Vector3d initial_velocity_estimate_; }; -class TranslationalEstimatorResults { - public: +class TranslationalEstimatorResults +{ +public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW // Constructor TranslationalEstimatorResults() - : position_measured_(Eigen::Vector3d::Zero()), - position_old_(Eigen::Vector3d::Zero()), - velocity_old_(Eigen::Vector3d::Zero()), - position_estimate_(Eigen::Vector3d::Zero()), - velocity_estimate_(Eigen::Vector3d::Zero()) {} + : position_measured_(Eigen::Vector3d::Zero()), + position_old_(Eigen::Vector3d::Zero()), + velocity_old_(Eigen::Vector3d::Zero()), + position_estimate_(Eigen::Vector3d::Zero()), + velocity_estimate_(Eigen::Vector3d::Zero()) + { + } // Intermediate Estimator results Eigen::Vector3d position_measured_; @@ -80,29 +91,26 @@ class TranslationalEstimatorResults { }; // Estimated object position and velocity from vicon data -class TranslationalEstimator { - public: +class TranslationalEstimator +{ +public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW // Constructor TranslationalEstimator(); // Update estimated quantities with new measurement - EstimatorStatus updateEstimate(const Eigen::Vector3d& pos_measured, - const double timestamp); + EstimatorStatus updateEstimate(const Eigen::Vector3d & pos_measured, const double timestamp); // Reset the estimator void reset(); // Setting the estimator parameters - void setParameters( - const TranslationalEstimatorParameters& estimator_parameters); + void setParameters(const TranslationalEstimatorParameters & estimator_parameters); // Return intermediate results structure - TranslationalEstimatorResults getResults() const { - return estimator_results_; - } + TranslationalEstimatorResults getResults() const { return estimator_results_; } // Return estimated position Eigen::Vector3d getEstimatedPosition() const { return position_estimate_W_; } // Return estimated velocity Eigen::Vector3d getEstimatedVelocity() const { return velocity_estimate_W_; } - private: +private: // Estimator parameters TranslationalEstimatorParameters estimator_parameters_; TranslationalEstimatorResults estimator_results_; @@ -124,16 +132,12 @@ static const double kDefaultdRateEstimateInitialCovariance = 1.0; static const double kDefaultdOrientationProcessCovariance = 0.01; static const double kDefaultdRateProcessCovariance = 1.0; static const double kDefaultOrientationMeasurementCovariance = 0.0005; -static const Eigen::Quaterniond kDefaultInitialOrientationEstimate = - Eigen::Quaterniond::Identity(); -static const Eigen::Vector3d kDefaultInitialRateEstimate = - Eigen::Vector3d::Zero(); -static const Eigen::Vector3d kDefaultInitialDorientationEstimate = - Eigen::Vector3d::Zero(); -static const Eigen::Vector3d kDefaultInitialDrateEstimate = - Eigen::Vector3d::Zero(); +static const Eigen::Quaterniond kDefaultInitialOrientationEstimate = Eigen::Quaterniond::Identity(); +static const Eigen::Vector3d kDefaultInitialRateEstimate = Eigen::Vector3d::Zero(); +static const Eigen::Vector3d kDefaultInitialDorientationEstimate = Eigen::Vector3d::Zero(); +static const Eigen::Vector3d kDefaultInitialDrateEstimate = Eigen::Vector3d::Zero(); static const OutlierRejectionMethod kDefaultOutlierRejectionMethod = - OutlierRejectionMethod::MAHALANOBIS_DISTANCE; + OutlierRejectionMethod::MAHALANOBIS_DISTANCE; static const double kDefaultOutlierRejectionMahalanobisThreshold = 5.0; @@ -142,31 +146,27 @@ static const int kDefaultOutlierRejectionSubsequentMaximumCount = 10.0; static const bool kOutputMinimalQuaternions = false; -class RotationalEstimatorParameters { - public: +class RotationalEstimatorParameters +{ +public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW // Constructor RotationalEstimatorParameters() - : dorientation_estimate_initial_covariance_( - kDefaultdOrientationEstimateInitialCovariance), - drate_estimate_initial_covariance_( - kDefaultdRateEstimateInitialCovariance), - dorientation_process_covariance_(kDefaultdOrientationProcessCovariance), - drate_process_covariance_(kDefaultdRateProcessCovariance), - orientation_measurement_covariance_( - kDefaultOrientationMeasurementCovariance), - initial_orientation_estimate_(kDefaultInitialOrientationEstimate), - initial_rate_estimate_(kDefaultInitialRateEstimate), - initial_dorientation_estimate_(kDefaultInitialDorientationEstimate), - initial_drate_estimate_(kDefaultInitialDrateEstimate), - outlier_rejection_method_(kDefaultOutlierRejectionMethod), - outlier_rejection_mahalanobis_threshold_( - kDefaultOutlierRejectionMahalanobisThreshold), - outlier_rejection_subsequent_threshold_degrees_( - kDefaultOutlierRejectionSubsequentThresholdDegrees), - outlier_rejection_subsequent_maximum_count_( - kDefaultOutlierRejectionSubsequentMaximumCount), - output_minimal_quaternions_(kOutputMinimalQuaternions){}; + : dorientation_estimate_initial_covariance_(kDefaultdOrientationEstimateInitialCovariance), + drate_estimate_initial_covariance_(kDefaultdRateEstimateInitialCovariance), + dorientation_process_covariance_(kDefaultdOrientationProcessCovariance), + drate_process_covariance_(kDefaultdRateProcessCovariance), + orientation_measurement_covariance_(kDefaultOrientationMeasurementCovariance), + initial_orientation_estimate_(kDefaultInitialOrientationEstimate), + initial_rate_estimate_(kDefaultInitialRateEstimate), + initial_dorientation_estimate_(kDefaultInitialDorientationEstimate), + initial_drate_estimate_(kDefaultInitialDrateEstimate), + outlier_rejection_method_(kDefaultOutlierRejectionMethod), + outlier_rejection_mahalanobis_threshold_(kDefaultOutlierRejectionMahalanobisThreshold), + outlier_rejection_subsequent_threshold_degrees_( + kDefaultOutlierRejectionSubsequentThresholdDegrees), + outlier_rejection_subsequent_maximum_count_(kDefaultOutlierRejectionSubsequentMaximumCount), + output_minimal_quaternions_(kOutputMinimalQuaternions){}; double dorientation_estimate_initial_covariance_; double drate_estimate_initial_covariance_; @@ -184,22 +184,23 @@ class RotationalEstimatorParameters { bool output_minimal_quaternions_; }; -class RotationalEstimatorResults { - public: +class RotationalEstimatorResults +{ +public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW // Constructor RotationalEstimatorResults() - : orientation_measured_(Eigen::Quaterniond::Identity()), - orientation_old_(Eigen::Quaterniond::Identity()), - rate_old_(Eigen::Vector3d::Zero()), - orientation_estimate_(Eigen::Quaterniond::Identity()), - rate_estimate_(Eigen::Vector3d::Zero()), - covariance_(Eigen::Matrix::Zero()), - measurement_outlier_flag_(false), - measurement_flip_flag_(false), - q_Z_Z1_magnitude_(0.0), - q_Z_B_mahalanobis_distance_(0.0), - q_covariance_trace_(0.0){}; + : orientation_measured_(Eigen::Quaterniond::Identity()), + orientation_old_(Eigen::Quaterniond::Identity()), + rate_old_(Eigen::Vector3d::Zero()), + orientation_estimate_(Eigen::Quaterniond::Identity()), + rate_estimate_(Eigen::Vector3d::Zero()), + covariance_(Eigen::Matrix::Zero()), + measurement_outlier_flag_(false), + measurement_flip_flag_(false), + q_Z_Z1_magnitude_(0.0), + q_Z_B_mahalanobis_distance_(0.0), + q_covariance_trace_(0.0){}; // Intermediate Estimator results Eigen::Quaterniond orientation_measured_; @@ -216,19 +217,19 @@ class RotationalEstimatorResults { }; // Estimated object orientation and roll rates from vicon data -class RotationalEstimator { - public: +class RotationalEstimator +{ +public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW // Constructor RotationalEstimator(); // Update estimated quantities with new measurement EstimatorStatus updateEstimate( - const Eigen::Quaterniond& orientation_measured_B_W, - const double timestamp); + const Eigen::Quaterniond & orientation_measured_B_W, const double timestamp); // Reset the estimator void reset(); // Setting the estimator parameters - void setParameters(const RotationalEstimatorParameters& estimator_parameters); + void setParameters(const RotationalEstimatorParameters & estimator_parameters); // Return intermediate results structure RotationalEstimatorResults getResults() const { return estimator_results_; } // Return estimated orientation @@ -236,7 +237,7 @@ class RotationalEstimator { // Return estimated angular velocity Eigen::Vector3d getEstimatedRate() const; - private: +private: // Estimator parameters RotationalEstimatorParameters estimator_parameters_; // Estimator (intermediate) results @@ -260,94 +261,94 @@ class RotationalEstimator { int outlier_counter_; // Function to generate a skew symmetric matrix from a vector - Eigen::Matrix3d skewMatrix(const Eigen::Vector3d& vec) const; + Eigen::Matrix3d skewMatrix(const Eigen::Vector3d & vec) const; // Serial of functions performing the estimate update steps void updateEstimatePropagateGlobalEstimate( - const Eigen::Matrix& x_old, const double dt, - Eigen::Matrix* x_priori); + const Eigen::Matrix & x_old, const double dt, + Eigen::Matrix * x_priori); void updateEstimatePropagateErrorEstimate( - const Eigen::Matrix& dx_old, - const Eigen::Matrix& x_old, const double dt, - Eigen::Matrix* dx_priori); + const Eigen::Matrix & dx_old, const Eigen::Matrix & x_old, + const double dt, Eigen::Matrix * dx_priori); void updateEstimatePropagateErrorCovariance( - Eigen::Matrix& cov_old, - const Eigen::Matrix& x_old, const double dt, - Eigen::Matrix* covariance_priori); + Eigen::Matrix & cov_old, const Eigen::Matrix & x_old, + const double dt, Eigen::Matrix * covariance_priori); void updateEstimateUpdateErrorEstimate( - const Eigen::Quaterniond& orientation_measured, - const Eigen::Matrix& x_priori, - const Eigen::Matrix& dx_priori, - const Eigen::Matrix& covariance_priori, - Eigen::Matrix* dx_measurement, - Eigen::Matrix* covariance_measurement); + const Eigen::Quaterniond & orientation_measured, const Eigen::Matrix & x_priori, + const Eigen::Matrix & dx_priori, + const Eigen::Matrix & covariance_priori, + Eigen::Matrix * dx_measurement, + Eigen::Matrix * covariance_measurement); void updateEstimateRecombineErrorGlobal( - const Eigen::Matrix x_priori, - Eigen::Matrix* x_measurement, - Eigen::Matrix* dx_measurement); + const Eigen::Matrix x_priori, Eigen::Matrix * x_measurement, + Eigen::Matrix * dx_measurement); // Checks if the estimator has crashed. bool checkForEstimatorCrash(); // Detects if the passed measurement is an outlier bool detectMeasurementOutlierMahalanobis( - const Eigen::Quaterniond& orientation_measured, - const Eigen::Matrix& covariance); - bool detectMeasurementOutlierSubsequent( - const Eigen::Quaterniond& orientation_measured); + const Eigen::Quaterniond & orientation_measured, + const Eigen::Matrix & covariance); + bool detectMeasurementOutlierSubsequent(const Eigen::Quaterniond & orientation_measured); // Returns the magnitude of the rotation represented by a quaternion - double quaternionRotationMagnitude(const Eigen::Quaterniond& rotation); + double quaternionRotationMagnitude(const Eigen::Quaterniond & rotation); // Ensure covariance symmetry - void makeCovarianceSymmetric(Eigen::Matrix* covariance); + void makeCovarianceSymmetric(Eigen::Matrix * covariance); }; -class ViconEstimator { - public: +class ViconEstimator +{ +public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW ViconEstimator(); // Update estimated quantities with new measurement - void updateEstimate(const Eigen::Vector3d& position_measured_W, - const Eigen::Quaterniond& orientation_measured_B_W, - const double timestamp); + void updateEstimate( + const Eigen::Vector3d & position_measured_W, + const Eigen::Quaterniond & orientation_measured_B_W, const double timestamp); // Reset the estimator void reset(); // Set estimator parameters void setParameters( - const TranslationalEstimatorParameters& - translational_estimator_parameters, - const RotationalEstimatorParameters& rotational_estimator_parameters); + const TranslationalEstimatorParameters & translational_estimator_parameters, + const RotationalEstimatorParameters & rotational_estimator_parameters); // Get intermediate results void getIntermediateResults( - TranslationalEstimatorResults* translational_estimator_results, - RotationalEstimatorResults* rotational_estimator_results) const; + TranslationalEstimatorResults * translational_estimator_results, + RotationalEstimatorResults * rotational_estimator_results) const; - void getEstimatorStatuses(EstimatorStatus* translational_estimator_status, - EstimatorStatus* rotational_estimator_status) const; + void getEstimatorStatuses( + EstimatorStatus * translational_estimator_status, + EstimatorStatus * rotational_estimator_status) const; // Functions providing access to the various estimates - Eigen::Vector3d getEstimatedPosition() const { + Eigen::Vector3d getEstimatedPosition() const + { return translational_estimator_.getEstimatedPosition(); } - Eigen::Vector3d getEstimatedVelocity() const { + Eigen::Vector3d getEstimatedVelocity() const + { return translational_estimator_.getEstimatedVelocity(); } - Eigen::Quaterniond getEstimatedOrientation() const { + Eigen::Quaterniond getEstimatedOrientation() const + { return rotational_estimator_.getEstimatedOrientation(); } - Eigen::Vector3d getEstimatedAngularVelocity() const { + Eigen::Vector3d getEstimatedAngularVelocity() const + { return rotational_estimator_.getEstimatedRate(); } - private: +private: TranslationalEstimator translational_estimator_; RotationalEstimator rotational_estimator_; EstimatorStatus translational_estimator_status_; EstimatorStatus rotational_estimator_status_; }; -} +} // namespace vicon_estimator #endif // VICON_ESTIMATOR_H diff --git a/include/vicon_odometry_estimator.h b/include/vicon_odometry_estimator.hpp similarity index 63% rename from include/vicon_odometry_estimator.h rename to include/vicon_odometry_estimator.hpp index 305bdfc..feccb6c 100644 --- a/include/vicon_odometry_estimator.h +++ b/include/vicon_odometry_estimator.hpp @@ -24,61 +24,62 @@ #include #include -#include -#include -#include #include +#include +#include -#include "vicon_estimator.h" +#include "rclcpp/rclcpp.hpp" +#include "ros_vrpn/msg/vicon_estimator.hpp" +#include "vicon_estimator.hpp" -namespace vicon_estimator { +namespace vicon_estimator +{ static const bool kDefaultVerboseFlag = true; -class ViconOdometryEstimator { - public: +class ViconOdometryEstimator : public rclcpp::Node +{ +public: // Constructor - ViconOdometryEstimator(ros::NodeHandle& nh); + ViconOdometryEstimator(const rclcpp::NodeOptions & options, const std::string & node_name="vicon_odometry_estimator"); // Initialize the estimator parameters - void initializeParameters(ros::NodeHandle& nh); + void initializeParameters(); // Reset the estimator void reset(); // Publishing the intermediate results - void publishIntermediateResults(ros::Time timestamp); + void publishIntermediateResults(rclcpp::Time timestamp); // Calls the underlying estimator, updating the estimate with the latest // measurement - void updateEstimate(const Eigen::Vector3d& position_measured_W, - const Eigen::Quaterniond& orientation_measured_B_W, - ros::Time timestamp); + void updateEstimate( + const Eigen::Vector3d & position_measured_W, + const Eigen::Quaterniond & orientation_measured_B_W, rclcpp::Time timestamp); // Getter methods for estimates values - Eigen::Vector3d getEstimatedPosition() const { - return vicon_estimator_.getEstimatedPosition(); - } + Eigen::Vector3d getEstimatedPosition() const { return vicon_estimator_.getEstimatedPosition(); } - Eigen::Vector3d getEstimatedVelocity() const { - return vicon_estimator_.getEstimatedVelocity(); - } + Eigen::Vector3d getEstimatedVelocity() const { return vicon_estimator_.getEstimatedVelocity(); } - Eigen::Quaterniond getEstimatedOrientation() const { + Eigen::Quaterniond getEstimatedOrientation() const + { return vicon_estimator_.getEstimatedOrientation(); } - Eigen::Vector3d getEstimatedAngularVelocity() const { + Eigen::Vector3d getEstimatedAngularVelocity() const + { return vicon_estimator_.getEstimatedAngularVelocity(); } - private: +private: // Underlying estimator vicon_estimator::ViconEstimator vicon_estimator_; // Publisher - ros::Publisher publisher_; + rclcpp::Publisher::SharedPtr publisher_; // Flag for verbose output bool verbose_; }; -} +} // namespace vicon_estimator #endif // VICON_ODOMETRY_ESTIMATOR_H diff --git a/launch/asl_optitrack.launch b/launch/asl_optitrack.launch index 1f1fce2..b455d9c 100644 --- a/launch/asl_optitrack.launch +++ b/launch/asl_optitrack.launch @@ -1,10 +1,10 @@ - + - + @@ -16,7 +16,7 @@ - + diff --git a/launch/asl_vicon.launch b/launch/asl_vicon.launch index 82edb7a..deac261 100644 --- a/launch/asl_vicon.launch +++ b/launch/asl_vicon.launch @@ -1,11 +1,11 @@ - + - + @@ -17,9 +17,9 @@ - + - + diff --git a/msg/viconEstimator.msg b/msg/ViconEstimator.msg similarity index 88% rename from msg/viconEstimator.msg rename to msg/ViconEstimator.msg index 8a21305..8f7d120 100644 --- a/msg/viconEstimator.msg +++ b/msg/ViconEstimator.msg @@ -1,4 +1,4 @@ -Header header +std_msgs/Header header geometry_msgs/Vector3 pos_measured # the measured body position geometry_msgs/Vector3 pos_old # the old body position @@ -18,5 +18,5 @@ std_msgs/Float64 q_covariance_trace std_msgs/Bool outlier_flag # flag indicating if the measurement at this timestep was detected as being an outlier std_msgs/Bool measurement_flip_flag # flag indicating if the measurement from vicon has undergone a redundant flipping. -std_msgs/Float64 q_Z_Z1_magnitude # the magnitude of the quaternion between subsequent measurements -std_msgs/Float64 q_Z_B_mahalanobis_distance # The mahalanobis distance of the current measurement +std_msgs/Float64 q_z_z1_magnitude # the magnitude of the quaternion between subsequent measurements +std_msgs/Float64 q_z_b_mahalanobis_distance # The mahalanobis distance of the current measurement diff --git a/package.xml b/package.xml index b11f0f0..5356ec5 100644 --- a/package.xml +++ b/package.xml @@ -1,6 +1,6 @@ - - ros_vrpn_client + + ros_vrpn 0.0.0 ROS node which is a client for Virtual Reality Peripheral @@ -15,21 +15,41 @@ New BSD - catkin - catkin_simple - message_generation + ament_cmake + rosidl_default_generators + eigen3_cmake_module + + eigen - roscpp + rclcpp + rclcpp_components + std_msgs tf2_ros tf2_geometry_msgs tf2_eigen geometry_msgs nav_msgs - vrpn_catkin + vrpn_vendor cmake_modules message_runtime - eigen_conversions - glog_catkin + + ament_lint_auto + ament_clang_format + ament_clang_tidy + + ament_cmake_cppcheck + ament_cmake_cpplint + ament_cmake_flake8 + ament_cmake_lint_cmake + ament_cmake_pep257 + + ament_cmake_xmllint + + rosidl_interface_packages + + + ament_cmake + diff --git a/src/library/vicon_estimator.cpp b/src/library/vicon_estimator.cpp index 3370119..57afa10 100644 --- a/src/library/vicon_estimator.cpp +++ b/src/library/vicon_estimator.cpp @@ -19,16 +19,18 @@ * limitations under the License. */ +#include "vicon_estimator.hpp" + #include #include #include -#include -#include +#include -#include "vicon_estimator.h" +#include "rcpputils/asserts.hpp" -namespace vicon_estimator { +namespace vicon_estimator +{ /* * -------------------------------------------------------------------- @@ -36,43 +38,46 @@ namespace vicon_estimator { * -------------------------------------------------------------------- */ -ViconEstimator::ViconEstimator() - : translational_estimator_(), rotational_estimator_() {} +ViconEstimator::ViconEstimator() : translational_estimator_(), rotational_estimator_() {} void ViconEstimator::updateEstimate( - const Eigen::Vector3d& position_measured_W, - const Eigen::Quaterniond& orientation_measured_B_W, - const double timestamp) { + const Eigen::Vector3d & position_measured_W, const Eigen::Quaterniond & orientation_measured_B_W, + const double timestamp) +{ // Updating the translational and rotation sub-estimates translational_estimator_status_ = - translational_estimator_.updateEstimate(position_measured_W, timestamp); + translational_estimator_.updateEstimate(position_measured_W, timestamp); rotational_estimator_status_ = - rotational_estimator_.updateEstimate(orientation_measured_B_W, timestamp); + rotational_estimator_.updateEstimate(orientation_measured_B_W, timestamp); } -void ViconEstimator::reset() { +void ViconEstimator::reset() +{ // Resetting the translational and rotation translational_estimator_.reset(); rotational_estimator_.reset(); } void ViconEstimator::setParameters( - const TranslationalEstimatorParameters& translational_estimator_parameters, - const RotationalEstimatorParameters& rotational_estimator_parameters) { + const TranslationalEstimatorParameters & translational_estimator_parameters, + const RotationalEstimatorParameters & rotational_estimator_parameters) +{ translational_estimator_.setParameters(translational_estimator_parameters); rotational_estimator_.setParameters(rotational_estimator_parameters); } void ViconEstimator::getIntermediateResults( - TranslationalEstimatorResults* translational_estimator_results, - RotationalEstimatorResults* rotational_estimator_results) const { + TranslationalEstimatorResults * translational_estimator_results, + RotationalEstimatorResults * rotational_estimator_results) const +{ *translational_estimator_results = translational_estimator_.getResults(); *rotational_estimator_results = rotational_estimator_.getResults(); } void ViconEstimator::getEstimatorStatuses( - EstimatorStatus* translational_estimator_status, - EstimatorStatus* rotational_estimator_status) const { + EstimatorStatus * translational_estimator_status, + EstimatorStatus * rotational_estimator_status) const +{ *translational_estimator_status = translational_estimator_status_; *rotational_estimator_status = rotational_estimator_status_; } @@ -83,14 +88,15 @@ void ViconEstimator::getEstimatorStatuses( * -------------------------------------------------------------------- */ -TranslationalEstimator::TranslationalEstimator() - : estimator_parameters_(), estimator_results_() { +TranslationalEstimator::TranslationalEstimator() : estimator_parameters_(), estimator_results_() +{ // Resetting the estimator reset(); } EstimatorStatus TranslationalEstimator::updateEstimate( - const Eigen::Vector3d& pos_measured_W, const double timestamp) { + const Eigen::Vector3d & pos_measured_W, const double timestamp) +{ // Performing some initialization if this is the first measurement. // Assuming first measurement valid, saving it and returning. if (first_measurement_flag_) { @@ -124,7 +130,7 @@ EstimatorStatus TranslationalEstimator::updateEstimate( // Constructing the Luenberger gain matrix Eigen::Matrix L_gain; L_gain << estimator_parameters_.kp_ * Eigen::Matrix3d::Identity(), - estimator_parameters_.kv_ * Eigen::Matrix3d::Identity(); + estimator_parameters_.kv_ * Eigen::Matrix3d::Identity(); // Correction using the Luenberger equations + gain x_estimate = (A - L_gain * C * A) * x_estimate + L_gain * pos_measured_W; // Extracting state components @@ -137,7 +143,8 @@ EstimatorStatus TranslationalEstimator::updateEstimate( return EstimatorStatus::OK; } -void TranslationalEstimator::reset() { +void TranslationalEstimator::reset() +{ // Resetting the estimates to their initial values position_estimate_W_ = estimator_parameters_.initial_position_estimate_; velocity_estimate_W_ = estimator_parameters_.initial_velocity_estimate_; @@ -147,7 +154,8 @@ void TranslationalEstimator::reset() { } void TranslationalEstimator::setParameters( - const TranslationalEstimatorParameters& estimator_parameters) { + const TranslationalEstimatorParameters & estimator_parameters) +{ estimator_parameters_ = estimator_parameters; } @@ -157,70 +165,63 @@ void TranslationalEstimator::setParameters( * -------------------------------------------------------------------- */ -RotationalEstimator::RotationalEstimator() - : estimator_parameters_(), estimator_results_() { +RotationalEstimator::RotationalEstimator() : estimator_parameters_(), estimator_results_() +{ // Resetting the estimator reset(); } -void RotationalEstimator::reset() { +void RotationalEstimator::reset() +{ // Resetting the estimates to thier initial values - orientation_estimate_B_W_ = - estimator_parameters_.initial_orientation_estimate_; + orientation_estimate_B_W_ = estimator_parameters_.initial_orientation_estimate_; rate_estimate_B_ = estimator_parameters_.initial_rate_estimate_; dorientation_estimate_ = estimator_parameters_.initial_rate_estimate_; drate_estimate_ = estimator_parameters_.initial_drate_estimate_; // Reseting to initial covariance - covariance_ - << estimator_parameters_.dorientation_estimate_initial_covariance_ * - Eigen::Matrix3d::Identity(), - Eigen::Matrix3d::Zero(), Eigen::Matrix3d::Zero(), - estimator_parameters_.drate_estimate_initial_covariance_ * - Eigen::Matrix3d::Identity(); + covariance_ << estimator_parameters_.dorientation_estimate_initial_covariance_ * + Eigen::Matrix3d::Identity(), + Eigen::Matrix3d::Zero(), Eigen::Matrix3d::Zero(), + estimator_parameters_.drate_estimate_initial_covariance_ * Eigen::Matrix3d::Identity(); // Constructing process and measurement covariance matrices - process_covariance_ - << estimator_parameters_.dorientation_process_covariance_ * - Eigen::Matrix3d::Identity(), - Eigen::Matrix3d::Zero(), Eigen::Matrix3d::Zero(), - estimator_parameters_.drate_process_covariance_ * - Eigen::Matrix3d::Identity(); - measurement_covariance_ - << estimator_parameters_.orientation_measurement_covariance_ * - Eigen::Matrix4d::Identity(); + process_covariance_ << estimator_parameters_.dorientation_process_covariance_ * + Eigen::Matrix3d::Identity(), + Eigen::Matrix3d::Zero(), Eigen::Matrix3d::Zero(), + estimator_parameters_.drate_process_covariance_ * Eigen::Matrix3d::Identity(); + measurement_covariance_ << estimator_parameters_.orientation_measurement_covariance_ * + Eigen::Matrix4d::Identity(); // Setting the old measurement to the intial position estimate last_timestamp_ = -1.0; - orientation_measured_old_ = - estimator_parameters_.initial_orientation_estimate_; + orientation_measured_old_ = estimator_parameters_.initial_orientation_estimate_; first_measurement_flag_ = true; outlier_counter_ = 0; } -void RotationalEstimator::setParameters( - const RotationalEstimatorParameters& estimator_parameters) { +void RotationalEstimator::setParameters(const RotationalEstimatorParameters & estimator_parameters) +{ estimator_parameters_ = estimator_parameters; } -Eigen::Quaterniond RotationalEstimator::getEstimatedOrientation() const { +Eigen::Quaterniond RotationalEstimator::getEstimatedOrientation() const +{ Eigen::Quaterniond orientation_for_return; // Swapping to minimal (but redundant) representation if requested if (estimator_parameters_.output_minimal_quaternions_) { double correction_factor = - orientation_estimate_B_W_.w() / std::abs(orientation_estimate_B_W_.w()); - orientation_for_return = Eigen::Quaterniond( - correction_factor * orientation_estimate_B_W_.coeffs()); + orientation_estimate_B_W_.w() / std::abs(orientation_estimate_B_W_.w()); + orientation_for_return = + Eigen::Quaterniond(correction_factor * orientation_estimate_B_W_.coeffs()); } else { orientation_for_return = orientation_estimate_B_W_; } return orientation_for_return; } -Eigen::Vector3d RotationalEstimator::getEstimatedRate() const { - return rate_estimate_B_; -} +Eigen::Vector3d RotationalEstimator::getEstimatedRate() const { return rate_estimate_B_; } EstimatorStatus RotationalEstimator::updateEstimate( - const Eigen::Quaterniond& orientation_measured_B_W, - const double timestamp) { + const Eigen::Quaterniond & orientation_measured_B_W, const double timestamp) +{ // Writing the raw measurement to the intermediate results structure estimator_results_.orientation_measured_ = orientation_measured_B_W; @@ -266,14 +267,14 @@ EstimatorStatus RotationalEstimator::updateEstimate( // Detecting outlier measurements bool measurement_update_flag; - if (estimator_parameters_.outlier_rejection_method_ == - OutlierRejectionMethod::MAHALANOBIS_DISTANCE) { - measurement_update_flag = - !detectMeasurementOutlierMahalanobis(orientation_measured_B_W, P_p); - } else if (estimator_parameters_.outlier_rejection_method_ == - OutlierRejectionMethod::SUBSEQUENT_MEASUREMENTS) { - measurement_update_flag = - !detectMeasurementOutlierSubsequent(orientation_measured_B_W); + if ( + estimator_parameters_.outlier_rejection_method_ == + OutlierRejectionMethod::MAHALANOBIS_DISTANCE) { + measurement_update_flag = !detectMeasurementOutlierMahalanobis(orientation_measured_B_W, P_p); + } else if ( + estimator_parameters_.outlier_rejection_method_ == + OutlierRejectionMethod::SUBSEQUENT_MEASUREMENTS) { + measurement_update_flag = !detectMeasurementOutlierSubsequent(orientation_measured_B_W); } else { measurement_update_flag = true; } @@ -281,8 +282,7 @@ EstimatorStatus RotationalEstimator::updateEstimate( // If no outlier detected do measurement update if (measurement_update_flag) { // Measurement Update - updateEstimateUpdateErrorEstimate(orientation_measured_B_W, x_p, dx_p, P_p, - &dx_m, &P_m); + updateEstimateUpdateErrorEstimate(orientation_measured_B_W, x_p, dx_p, P_p, &dx_m, &P_m); // Global state correction (combining priori global state estimate with // posteriori error state estimate) updateEstimateRecombineErrorGlobal(x_p, &x_m, &dx_m); @@ -329,30 +329,28 @@ EstimatorStatus RotationalEstimator::updateEstimate( } } -Eigen::Matrix3d RotationalEstimator::skewMatrix( - const Eigen::Vector3d& vec) const { +Eigen::Matrix3d RotationalEstimator::skewMatrix(const Eigen::Vector3d & vec) const +{ Eigen::Matrix3d vec_cross; vec_cross << 0, -vec(2), vec(1), vec(2), 0, -vec(0), -vec(1), vec(0), 0; return vec_cross; } void RotationalEstimator::updateEstimatePropagateGlobalEstimate( - const Eigen::Matrix& x_old, const double dt, - Eigen::Matrix* x_p) { + const Eigen::Matrix & x_old, const double dt, Eigen::Matrix * x_p) +{ // Argument checks - CHECK_NOTNULL(x_p); + rcpputils::require_true(x_p != nullptr, "x_p is nullptr"); // Extracting components of the state - Eigen::Quaterniond orienation_estimate_old = - Eigen::Quaterniond(x_old.block<4, 1>(0, 0)); + Eigen::Quaterniond orienation_estimate_old = Eigen::Quaterniond(x_old.block<4, 1>(0, 0)); Eigen::Matrix rate_estimate_old = x_old.block<3, 1>(4, 0); // Converting the roll rate vector to quaternion representation - Eigen::Quaterniond rate_estimate_quaternion = Eigen::Quaterniond( - 0, rate_estimate_old.x(), rate_estimate_old.y(), rate_estimate_old.z()); + Eigen::Quaterniond rate_estimate_quaternion = + Eigen::Quaterniond(0, rate_estimate_old.x(), rate_estimate_old.y(), rate_estimate_old.z()); // Performing orientation and roll rate propagation Eigen::Quaterniond orientation_estimate_priori = Eigen::Quaterniond( - orienation_estimate_old.coeffs() + - (0.5 * (orienation_estimate_old * rate_estimate_quaternion).coeffs()) * - dt); + orienation_estimate_old.coeffs() + + (0.5 * (orienation_estimate_old * rate_estimate_quaternion).coeffs()) * dt); Eigen::Vector3d rate_estimate_priori = rate_estimate_old; // Renormalizing the quaternion orientation_estimate_priori.normalize(); @@ -361,91 +359,81 @@ void RotationalEstimator::updateEstimatePropagateGlobalEstimate( } void RotationalEstimator::updateEstimatePropagateErrorEstimate( - const Eigen::Matrix& dx_old, - const Eigen::Matrix& x_old, const double dt, - Eigen::Matrix* dx_p) { + const Eigen::Matrix & dx_old, const Eigen::Matrix & x_old, + const double dt, Eigen::Matrix * dx_p) +{ // Argument checks - CHECK_NOTNULL(dx_p); + rcpputils::require_true(dx_p != nullptr, "dx_p is nullptr"); // Extracting components of the states - Eigen::Quaterniond orienation_estimate = - Eigen::Quaterniond(x_old.block<4, 1>(0, 0)); + Eigen::Quaterniond orienation_estimate = Eigen::Quaterniond(x_old.block<4, 1>(0, 0)); Eigen::Matrix rate_estimate = x_old.block<3, 1>(4, 0); Eigen::Matrix dorientation_estimate = dx_old.block<3, 1>(0, 0); Eigen::Matrix drate_estimate = dx_old.block<3, 1>(3, 0); // Performing propagation Eigen::Vector3d dorientation_estimate_priori_roc = - -rate_estimate.cross(dorientation_estimate) + - 0.5 * drate_estimate; // Appears to agree with equations in papers + -rate_estimate.cross(dorientation_estimate) + + 0.5 * drate_estimate; // Appears to agree with equations in papers Eigen::Vector3d drate_estimate_priori_roc = Eigen::Vector3d::Zero(); Eigen::Vector3d dorientation_estimate_priori = - dorientation_estimate + dorientation_estimate_priori_roc * dt; - Eigen::Vector3d drate_estimate_priori = - drate_estimate + drate_estimate_priori_roc * dt; + dorientation_estimate + dorientation_estimate_priori_roc * dt; + Eigen::Vector3d drate_estimate_priori = drate_estimate + drate_estimate_priori_roc * dt; // Writing to apriori error state *dx_p << dorientation_estimate_priori, drate_estimate_priori; } void RotationalEstimator::updateEstimatePropagateErrorCovariance( - Eigen::Matrix& covariance_old, - const Eigen::Matrix& x_old, const double dt, - Eigen::Matrix* covariance_priori) { + Eigen::Matrix & covariance_old, const Eigen::Matrix & x_old, + const double dt, Eigen::Matrix * covariance_priori) +{ // Argument checks - CHECK_NOTNULL(covariance_priori); + rcpputils::require_true(covariance_priori != nullptr, "covariance_priori is nullptr"); // Extracting components of the state - Eigen::Quaterniond orientation_estimate = - Eigen::Quaterniond(x_old.block<4, 1>(0, 0)); + Eigen::Quaterniond orientation_estimate = Eigen::Quaterniond(x_old.block<4, 1>(0, 0)); Eigen::Matrix rate_estimate = x_old.block<3, 1>(4, 0); // Constructing linearized system matrices Eigen::Matrix A; Eigen::Matrix L; - A << -1 * skewMatrix(rate_estimate), 0.5 * Eigen::Matrix3d::Identity(), - Eigen::Matrix3d::Zero(), Eigen::Matrix3d::Zero(); - L << Eigen::Matrix3d::Identity(), Eigen::Matrix3d::Zero(), - Eigen::Matrix3d::Zero(), Eigen::Matrix3d::Identity(); + A << -1 * skewMatrix(rate_estimate), 0.5 * Eigen::Matrix3d::Identity(), Eigen::Matrix3d::Zero(), + Eigen::Matrix3d::Zero(); + L << Eigen::Matrix3d::Identity(), Eigen::Matrix3d::Zero(), Eigen::Matrix3d::Zero(), + Eigen::Matrix3d::Identity(); // Performing propagation ( P_kp1=A_d * P_k * A_d' + L * Qd * L' ) Eigen::Matrix covariance_priori_propagation; Eigen::Matrix covariance_priori_addition; - Eigen::Matrix A_d = - Eigen::Matrix::Identity() + A * dt; + Eigen::Matrix A_d = Eigen::Matrix::Identity() + A * dt; covariance_priori_propagation = A_d * covariance_old * A_d.transpose(); covariance_priori_addition = L * process_covariance_ * (L.transpose()) * dt; - *covariance_priori = - covariance_priori_propagation + covariance_priori_addition; + *covariance_priori = covariance_priori_propagation + covariance_priori_addition; // Ensuring symmetry and writing output makeCovarianceSymmetric(covariance_priori); } void RotationalEstimator::updateEstimateUpdateErrorEstimate( - const Eigen::Quaterniond& orientation_measured, - const Eigen::Matrix& x_priori, - const Eigen::Matrix& dx_priori, - const Eigen::Matrix& covariance_priori, - Eigen::Matrix* dx_m, - Eigen::Matrix* covariance_measurement) { + const Eigen::Quaterniond & orientation_measured, const Eigen::Matrix & x_priori, + const Eigen::Matrix & dx_priori, + const Eigen::Matrix & covariance_priori, Eigen::Matrix * dx_m, + Eigen::Matrix * covariance_measurement) +{ // Argument checks - CHECK_NOTNULL(dx_m); - CHECK_NOTNULL(covariance_measurement); + rcpputils::require_true(dx_m != nullptr, "dx_m is nullptr"); + rcpputils::require_true(covariance_measurement != nullptr, "covariance_measurement is nullptr"); + // Extracting components of the state - Eigen::Quaterniond orientation_estimate_priori = - Eigen::Quaterniond(x_priori.block<4, 1>(0, 0)); + Eigen::Quaterniond orientation_estimate_priori = Eigen::Quaterniond(x_priori.block<4, 1>(0, 0)); Eigen::Matrix rate_estimate_priori = x_priori.block<3, 1>(4, 0); - Eigen::Matrix dorientation_estimate_priori = - dx_priori.block<3, 1>(0, 0); - Eigen::Matrix drate_estimate_priori = - dx_priori.block<3, 1>(3, 0); + Eigen::Matrix dorientation_estimate_priori = dx_priori.block<3, 1>(0, 0); + Eigen::Matrix drate_estimate_priori = dx_priori.block<3, 1>(3, 0); // Constructing linearized measurement matrix Eigen::Matrix Hdq; Eigen::Matrix H; - Eigen::Vector3d orientation_estimate_priori_vector = - orientation_estimate_priori.vec(); - Hdq << orientation_estimate_priori.w() * - Eigen::Matrix::Identity() + - skewMatrix(orientation_estimate_priori_vector), - -orientation_estimate_priori.vec().transpose(); + Eigen::Vector3d orientation_estimate_priori_vector = orientation_estimate_priori.vec(); + Hdq << orientation_estimate_priori.w() * Eigen::Matrix::Identity() + + skewMatrix(orientation_estimate_priori_vector), + -orientation_estimate_priori.vec().transpose(); H << Hdq, Eigen::Matrix::Zero(); // Calculating the measured error quaternion Eigen::Quaterniond error_orientation = - orientation_measured * orientation_estimate_priori.inverse(); + orientation_measured * orientation_estimate_priori.inverse(); // Calculating the predicted measurement dependant on the sign of the measured // error quaternion Eigen::Quaterniond orientation_predicted; @@ -454,23 +442,19 @@ void RotationalEstimator::updateEstimateUpdateErrorEstimate( estimator_results_.measurement_flip_flag_ = false; // Calculating the predicted measurement orientation_predicted = - Eigen::Quaterniond(Hdq * dorientation_estimate_priori + - orientation_estimate_priori.coeffs()); + Eigen::Quaterniond(Hdq * dorientation_estimate_priori + orientation_estimate_priori.coeffs()); } else { // Assigning the flag indicating that this measurment is flipped estimator_results_.measurement_flip_flag_ = true; // Calculating the predicted measurement orientation_predicted = - Eigen::Quaterniond(Hdq * dorientation_estimate_priori + - orientation_estimate_priori.coeffs()); + Eigen::Quaterniond(Hdq * dorientation_estimate_priori + orientation_estimate_priori.coeffs()); } // Calculating the measurement residual Eigen::Vector4d measurement_residual; - measurement_residual = - orientation_measured.coeffs() - orientation_predicted.coeffs(); + measurement_residual = orientation_measured.coeffs() - orientation_predicted.coeffs(); // Computing the Kalman gain - Eigen::Matrix S = - H * covariance_priori * H.transpose() + measurement_covariance_; + Eigen::Matrix S = H * covariance_priori * H.transpose() + measurement_covariance_; Eigen::Matrix b = H * covariance_priori; Eigen::Matrix A = S.transpose(); Eigen::Matrix x = A.colPivHouseholderQr().solve(b); @@ -478,79 +462,70 @@ void RotationalEstimator::updateEstimateUpdateErrorEstimate( // Correcting the state *dx_m = dx_priori + K * measurement_residual; // Updating the covariance - *covariance_measurement = - (Eigen::Matrix::Identity() - K * H) * covariance_priori; + *covariance_measurement = (Eigen::Matrix::Identity() - K * H) * covariance_priori; // Making the covariance matrix symmetric makeCovarianceSymmetric(covariance_measurement); } void RotationalEstimator::updateEstimateRecombineErrorGlobal( - const Eigen::Matrix x_priori, - Eigen::Matrix* x_measurement, - Eigen::Matrix* dx_measurement) { + const Eigen::Matrix x_priori, Eigen::Matrix * x_measurement, + Eigen::Matrix * dx_measurement) +{ // Argument checks - CHECK_NOTNULL(x_measurement); - CHECK_NOTNULL(dx_measurement); + rcpputils::require_true(x_measurement != nullptr, "x_measurement is nullptr"); + rcpputils::require_true(dx_measurement != nullptr, "dx_measurement is nullptr"); + // Extracting components of the state - Eigen::Quaterniond orientation_estimate_priori = - Eigen::Quaterniond(x_priori.block<4, 1>(0, 0)); + Eigen::Quaterniond orientation_estimate_priori = Eigen::Quaterniond(x_priori.block<4, 1>(0, 0)); Eigen::Matrix rate_estimate_priori = x_priori.block<3, 1>(4, 0); - Eigen::Matrix dorientation_estimate_measurement = - dx_measurement->block<3, 1>(0, 0); - Eigen::Matrix drate_estimate_measurement = - dx_measurement->block<3, 1>(3, 0); + Eigen::Matrix dorientation_estimate_measurement = dx_measurement->block<3, 1>(0, 0); + Eigen::Matrix drate_estimate_measurement = dx_measurement->block<3, 1>(3, 0); // Completing the error quaternion // The sign real part of the full quaternion was calculated in the error // quaternion measurement update step Eigen::Quaterniond dorientation_estimate_measurement_quaternion; if (estimator_results_.measurement_flip_flag_ == false) { - dorientation_estimate_measurement_quaternion = - Eigen::Quaterniond(1.0, dorientation_estimate_measurement.x(), - dorientation_estimate_measurement.y(), - dorientation_estimate_measurement.z()); + dorientation_estimate_measurement_quaternion = Eigen::Quaterniond( + 1.0, dorientation_estimate_measurement.x(), dorientation_estimate_measurement.y(), + dorientation_estimate_measurement.z()); } else { - dorientation_estimate_measurement_quaternion = - Eigen::Quaterniond(-1.0, dorientation_estimate_measurement.x(), - dorientation_estimate_measurement.y(), - dorientation_estimate_measurement.z()); + dorientation_estimate_measurement_quaternion = Eigen::Quaterniond( + -1.0, dorientation_estimate_measurement.x(), dorientation_estimate_measurement.y(), + dorientation_estimate_measurement.z()); } // Using estimated error states to correct global estimate states Eigen::Quaterniond orientation_estimate_measurement = - orientation_estimate_priori * - dorientation_estimate_measurement_quaternion; + orientation_estimate_priori * dorientation_estimate_measurement_quaternion; Eigen::Matrix rate_estimate_measurement = - rate_estimate_priori + drate_estimate_measurement; + rate_estimate_priori + drate_estimate_measurement; // Normalizing the posteriori quaternion orientation_estimate_measurement.normalize(); // Writing to posteriori global state and error states - *x_measurement << orientation_estimate_measurement.coeffs(), - rate_estimate_measurement; + *x_measurement << orientation_estimate_measurement.coeffs(), rate_estimate_measurement; *dx_measurement = Eigen::Matrix::Zero(); } -bool RotationalEstimator::checkForEstimatorCrash() { +bool RotationalEstimator::checkForEstimatorCrash() +{ // Testing if covariance matrix member is nan. double nan_test = covariance_.sum(); return std::isnan(nan_test); } bool RotationalEstimator::detectMeasurementOutlierMahalanobis( - const Eigen::Quaterniond& orientation_measured, - const Eigen::Matrix& covariance) { + const Eigen::Quaterniond & orientation_measured, const Eigen::Matrix & covariance) +{ // Initializing the outlier flag bool measurement_outlier_flag = false; // Calculating the mahalanobis distance (3x3 matrix - Should be fine for // direct inverse). - Eigen::Quaterniond q_Z_B = - orientation_measured * orientation_estimate_B_W_.inverse(); + Eigen::Quaterniond q_Z_B = orientation_measured * orientation_estimate_B_W_.inverse(); Eigen::Vector3d dq_Z_B_ = q_Z_B.vec(); Eigen::Matrix dq_covariance = covariance.block<3, 3>(0, 0); - double q_Z_B_mahalanobis_distance = - sqrt(dq_Z_B_.transpose() * dq_covariance.inverse() * dq_Z_B_); + double q_Z_B_mahalanobis_distance = sqrt(dq_Z_B_.transpose() * dq_covariance.inverse() * dq_Z_B_); // Detecting outlier measurement_outlier_flag = - q_Z_B_mahalanobis_distance >= - estimator_parameters_.outlier_rejection_mahalanobis_threshold_; + q_Z_B_mahalanobis_distance >= estimator_parameters_.outlier_rejection_mahalanobis_threshold_; // Writing the intermediate results (for debug) estimator_results_.q_Z_Z1_magnitude_ = -1.0; estimator_results_.q_Z_B_mahalanobis_distance_ = q_Z_B_mahalanobis_distance; @@ -569,24 +544,22 @@ bool RotationalEstimator::detectMeasurementOutlierMahalanobis( } bool RotationalEstimator::detectMeasurementOutlierSubsequent( - const Eigen::Quaterniond& orientation_measured) { + const Eigen::Quaterniond & orientation_measured) +{ // Initializing the outlier flag bool measurement_outlier_flag = false; // Constructing the quaternion representing the rotation between subsquent // measurements - Eigen::Quaterniond q_Z_Z1 = - orientation_measured * orientation_measured_old_.inverse(); + Eigen::Quaterniond q_Z_Z1 = orientation_measured * orientation_measured_old_.inverse(); // Calculating the quaternion magnitude double q_Z_Z1_magnitude = quaternionRotationMagnitude(q_Z_Z1); // Detecting outlier measurement_outlier_flag = - q_Z_Z1_magnitude >= - estimator_parameters_.outlier_rejection_subsequent_threshold_degrees_ * - M_PI / 180.0; + q_Z_Z1_magnitude >= + estimator_parameters_.outlier_rejection_subsequent_threshold_degrees_ * M_PI / 180.0; // After a certain number of measurements have been ignored in a row // we assume we've made a mistake and accept the measurement as valid. - if (outlier_counter_ >= - estimator_parameters_.outlier_rejection_subsequent_maximum_count_) { + if (outlier_counter_ >= estimator_parameters_.outlier_rejection_subsequent_maximum_count_) { measurement_outlier_flag = false; } // Writing the intermediate results (for debug) @@ -604,8 +577,8 @@ bool RotationalEstimator::detectMeasurementOutlierSubsequent( } } -double RotationalEstimator::quaternionRotationMagnitude( - const Eigen::Quaterniond& rotation) { +double RotationalEstimator::quaternionRotationMagnitude(const Eigen::Quaterniond & rotation) +{ // Extracting the quaternion magnitude component double positive_rotation_return = 2 * acos(rotation.w()); double negative_rotation_return = 2 * M_PI - 2 * acos(rotation.w()); @@ -616,10 +589,11 @@ double RotationalEstimator::quaternionRotationMagnitude( } } -void RotationalEstimator::makeCovarianceSymmetric( - Eigen::Matrix* covariance) { +void RotationalEstimator::makeCovarianceSymmetric(Eigen::Matrix * covariance) +{ // Argument checks - CHECK_NOTNULL(covariance); + rcpputils::require_true(covariance != nullptr, "covariance is nullptr"); + *covariance = (*covariance + covariance->transpose()) / 2; } -} +} // namespace vicon_estimator diff --git a/src/ros_vrpn_client.cpp b/src/ros_vrpn_client.cpp index 5db807c..6b324e9 100644 --- a/src/ros_vrpn_client.cpp +++ b/src/ros_vrpn_client.cpp @@ -39,24 +39,22 @@ // estimated // quantities and raw data are then published through ROS. -#include -#include #include -#include - -#include -#include +#include +#include #include + #include -#include -#include -#include -#include -#include +#include +#include +#include +#include +#include -#include "vicon_odometry_estimator.h" +#include "vicon_odometry_estimator.hpp" +#include "vrpn_Connection.h" -void VRPN_CALLBACK track_target(void*, const vrpn_TRACKERCB tracker); +void VRPN_CALLBACK track_target(void *, const vrpn_TRACKERCB tracker); // NOTE(millanea@ethz.ch): The following callbacks should be implemented in the // case that @@ -70,55 +68,109 @@ void VRPN_CALLBACK track_target(void*, const vrpn_TRACKERCB tracker); // ta); // A class representing the state of the tracked target. -struct TargetState { - geometry_msgs::TransformStamped measured_transform; - geometry_msgs::TransformStamped estimated_transform; - nav_msgs::Odometry estimated_odometry; +struct TargetState +{ + geometry_msgs::msg::TransformStamped measured_transform; + geometry_msgs::msg::TransformStamped estimated_transform; + nav_msgs::msg::Odometry estimated_odometry; }; // Available coordinate systems. -enum CoordinateSystem { kVicon, kOptitrack } coordinate_system; +enum CoordinateSystem +{ + kVicon, + kOptitrack +}; // Available timestamping options. -enum TimestampingSystem { kTrackerStamp, kRosStamp } timestamping_system; +enum TimestampingSystem +{ + kTrackerStamp, + kRosStamp +}; // Global target descriptions. -TargetState* target_state; -std::string object_name; -std::string coordinate_system_string; +TargetState * target_state; // Global indicating the availability of new VRPN callback function. bool fresh_data = false; vrpn_TRACKERCB prev_tracker; // Global indicating if we should display the time delays -bool display_time_delay = true; // Pointer to the vicon estimator. Global such that it can be accessed from the // callback. -vicon_estimator::ViconOdometryEstimator* vicon_odometry_estimator = NULL; +std::shared_ptr vicon_odometry_estimator; -class Rigid_Body { - public: +class Rigid_Body : public rclcpp::Node +{ +public: // Constructor - Rigid_Body(ros::NodeHandle& nh, std::string server_ip, int port, - const std::string& object_name) { + Rigid_Body(const rclcpp::NodeOptions & options) : Node("rigid_body", options) + { + // Retrieving control parameters + this->declare_parameter("vrpn_server_ip", "vicon"); + this->get_parameter("vrpn_server_ip", vrpn_server_ip); + this->declare_parameter("vrpn_port", 3883); + this->get_parameter("vrpn_port", vrpn_port); + this->declare_parameter("vrpn_coordinate_system", "vicon"); + this->get_parameter("vrpn_coordinate_system", coordinate_system_string); + this->declare_parameter("object_name", "auk"); + this->get_parameter("object_name", object_name); + this->declare_parameter("timestamping_system", "tracker"); + this->get_parameter("timestamping_system", timestamping_system_string); + this->declare_parameter("display_time_delay", true); + this->get_parameter("display_time_delay", display_time_delay); + + // Debug output + std::cout << "vrpn_server_ip:" << vrpn_server_ip << std::endl; + std::cout << "vrpn_port:" << vrpn_port << std::endl; + std::cout << "vrpn_coordinate_system:" << coordinate_system_string << std::endl; + std::cout << "object_name:" << object_name << std::endl; + std::cout << "timestamping_system:" << timestamping_system_string << std::endl; + + // Setting the coordinate system based on the ros param + if (coordinate_system_string == "vicon") { + coordinate_system = CoordinateSystem::kVicon; + } else if (coordinate_system_string == "optitrack") { + coordinate_system = CoordinateSystem::kOptitrack; + } else { + RCLCPP_FATAL( + this->get_logger(), + "ROS param vrpn_coordinate_system should be either 'vicon' or " + "'optitrack'!"); + rclcpp::shutdown(); + } + + // Setting the time stamping option based on the ros param + if (timestamping_system_string == "tracker") { + timestamping_system = TimestampingSystem::kTrackerStamp; + } else if (timestamping_system_string == "ros") { + timestamping_system = TimestampingSystem::kRosStamp; + } else { + RCLCPP_FATAL( + this->get_logger(), "ROS param timestamping_system should be either 'tracker' or 'ros'!"); + rclcpp::shutdown(); + } + // Advertising published topics. measured_target_transform_pub_ = - nh.advertise("raw_transform", 1); + create_publisher("raw_transform", 1); estimated_target_transform_pub_ = - nh.advertise("estimated_transform", 1); + create_publisher("estimated_transform", 1); estimated_target_odometry_pub_ = - nh.advertise("estimated_odometry", 1); + create_publisher("estimated_odometry", 1); + br = std::make_unique(*this); + // Connecting to the vprn device and creating an associated tracker. std::stringstream connection_name; - connection_name << server_ip << ":" << port; + connection_name << vrpn_server_ip << ":" << vrpn_port; connection = vrpn_get_connection_by_name(connection_name.str().c_str()); tracker = new vrpn_Tracker_Remote(object_name.c_str(), connection); tracker->print_latest_report(); // Registering a callback to be called when a new data is made available by // the vrpn sever. - this->tracker->register_change_handler(NULL, track_target); + this->tracker->register_change_handler(this, track_target); tracker->print_latest_report(); // NOTE(millanea@ethz.ch): The following callbacks should be added if // they're available. @@ -128,37 +180,53 @@ class Rigid_Body { } // Publishes the raw measured target state to the transform message. - void publish_measured_transform(TargetState* target_state) { - measured_target_transform_pub_.publish(target_state->measured_transform); + void publish_measured_transform(TargetState * target_state) + { + measured_target_transform_pub_->publish(target_state->measured_transform); } // Publishes the estimated target state to the transform message and sends // tranform. - void publish_estimated_transform(TargetState* target_state) { - br.sendTransform(target_state->estimated_transform); - estimated_target_transform_pub_.publish(target_state->estimated_transform); + void publish_estimated_transform(TargetState * target_state) + { + br->sendTransform(target_state->estimated_transform); + estimated_target_transform_pub_->publish(target_state->estimated_transform); } // Publishes the estimated target state to the odometry message. - void publish_estimated_odometry(TargetState* target_state) { - estimated_target_odometry_pub_.publish(target_state->estimated_odometry); + void publish_estimated_odometry(TargetState * target_state) + { + estimated_target_odometry_pub_->publish(target_state->estimated_odometry); } // Passes contol to the vrpn client. - void step_vrpn() { + void step_vrpn() + { this->tracker->mainloop(); this->connection->mainloop(); } - private: + // Public Params: + std::string vrpn_server_ip; + int vrpn_port; + std::string trackedObjectName; + std::string timestamping_system_string; + bool display_time_delay = true; + TimestampingSystem timestamping_system; + CoordinateSystem coordinate_system; + std::string coordinate_system_string; + std::string object_name; + +private: // Publishers - ros::Publisher measured_target_transform_pub_; - ros::Publisher estimated_target_transform_pub_; - ros::Publisher estimated_target_odometry_pub_; - tf2_ros::TransformBroadcaster br; + rclcpp::Publisher::SharedPtr measured_target_transform_pub_; + rclcpp::Publisher::SharedPtr + estimated_target_transform_pub_; + rclcpp::Publisher::SharedPtr estimated_target_odometry_pub_; + std::unique_ptr br; // Vprn object pointers - vrpn_Connection* connection; - vrpn_Tracker_Remote* tracker; + vrpn_Connection * connection; + vrpn_Tracker_Remote * tracker; }; // TODO(millanea@ethz.ch): The following callbacks should be implemented if @@ -179,23 +247,20 @@ class Rigid_Body { // Corrects measured target orientation and position for differing frame // definitions. void inline correctForCoordinateSystem( - const Eigen::Quaterniond& orientation_in, - const Eigen::Vector3d& position_in, CoordinateSystem coordinate_system, - Eigen::Quaterniond* orientation_measured_B_W, - Eigen::Vector3d* position_measured_W) { + const rclcpp::Node * nh, const Eigen::Quaterniond & orientation_in, + const Eigen::Vector3d & position_in, CoordinateSystem coordinate_system, + Eigen::Quaterniond * orientation_measured_B_W, Eigen::Vector3d * position_measured_W) +{ // Rotation to correct between optitrack and vicon - const Eigen::Quaterniond kqOptitrackFix( - Eigen::AngleAxisd(0.5 * M_PI, Eigen::Vector3d::UnitX())); + const Eigen::Quaterniond kqOptitrackFix(Eigen::AngleAxisd(0.5 * M_PI, Eigen::Vector3d::UnitX())); // Correcting measurements based on coordinate system switch (coordinate_system) { case kOptitrack: { // Here we rotate the Optitrack measured quaternion by qFix, a // Pi/2 rotation around the x-axis. By doing so we convert from // NED to ENU. - *orientation_measured_B_W = - kqOptitrackFix* orientation_in* kqOptitrackFix.inverse(); - *position_measured_W = - Eigen::Vector3d(position_in.x(), -position_in.z(), position_in.y()); + *orientation_measured_B_W = kqOptitrackFix * orientation_in * kqOptitrackFix.inverse(); + *position_measured_W = Eigen::Vector3d(position_in.x(), -position_in.z(), position_in.y()); break; } case kVicon: { @@ -204,13 +269,15 @@ void inline correctForCoordinateSystem( break; } default: { - ROS_FATAL("Coordinate system not defined!"); + RCLCPP_FATAL(nh->get_logger(), "Coordinate system not defined!"); break; } } } -void inline getTimeStamp(const ros::Time& vicon_stamp, ros::Time* timestamp) { +void inline getTimeStamp( + const Rigid_Body * nh, const rclcpp::Time & vicon_stamp, rclcpp::Time * timestamp) +{ // Stamping message depending on selected stamping source // tracker: Use stamp attached to the vrpn_client callback comming from the // tracker system. Not that this timestamp may not be synced to the @@ -219,91 +286,90 @@ void inline getTimeStamp(const ros::Time& vicon_stamp, ros::Time* timestamp) { // in the tracker software. These delay hours are removed in this // function. // ros: Stamp the message on arrival with the current ros time. - switch (timestamping_system) { + switch (nh->timestamping_system) { case kTrackerStamp: { // Retreiving current ROS Time - ros::Time ros_stamp = ros::Time::now(); + rclcpp::Time ros_stamp = nh->get_clock()->now(); // Calculating the difference between the tracker attached timestamp and // the current ROS time. - ros::Duration time_diff = vicon_stamp - ros_stamp; + rclcpp::Duration time_diff = vicon_stamp - ros_stamp; // Working out the hours difference in hours and then rounding to the // closest hour const double kHoursToSec = 3600; - double time_diff_h = time_diff.toSec() / kHoursToSec; + double time_diff_h = time_diff.seconds() / kHoursToSec; double time_diff_h_round = std::round(time_diff_h); // Correcting the time stamp by the hours difference and reassembling // timestamp double time_correction_s = time_diff_h_round * kHoursToSec; - ros::Time vicon_stamp_corrected(vicon_stamp.sec - time_correction_s, - vicon_stamp.nsec); + rclcpp::Time vicon_stamp_corrected( + vicon_stamp.seconds() - time_correction_s, vicon_stamp.nanoseconds()); // Attaching the corrected timestamp *timestamp = vicon_stamp_corrected; // Outputting the time delay to the ROS console - if (display_time_delay) { - ros::Duration time_diff_corrected = ros_stamp - vicon_stamp_corrected; + if (nh->display_time_delay) { + rclcpp::Duration time_diff_corrected = ros_stamp - vicon_stamp_corrected; static const int kMaxMessagePeriod = 2; - ROS_WARN_STREAM_THROTTLE(kMaxMessagePeriod, - "Time delay: " << time_diff_corrected.toSec()); + RCLCPP_WARN_STREAM_THROTTLE( + nh->get_logger(), *nh->get_clock(), kMaxMessagePeriod, + "Time delay: " << time_diff_corrected.seconds()); } break; } case kRosStamp: { // Just attach the current ROS timestamp - *timestamp = ros::Time::now(); + *timestamp = nh->get_clock()->now(); break; } } } // Compares two instances of tracker data for equality -bool inline tracker_is_equal(const vrpn_TRACKERCB& vprn_data_1, - const vrpn_TRACKERCB& vprn_data_2) { - return (vprn_data_1.quat[0] == vprn_data_2.quat[0] and - vprn_data_1.quat[1] == vprn_data_2.quat[1] and - vprn_data_1.quat[2] == vprn_data_2.quat[2] and - vprn_data_1.quat[3] == vprn_data_2.quat[3] and - vprn_data_1.pos[0] == vprn_data_2.pos[0] and - vprn_data_1.pos[1] == vprn_data_2.pos[1] and - vprn_data_1.pos[2] == vprn_data_2.pos[2]); +bool inline tracker_is_equal(const vrpn_TRACKERCB & vprn_data_1, const vrpn_TRACKERCB & vprn_data_2) +{ + return ( + vprn_data_1.quat[0] == vprn_data_2.quat[0] and vprn_data_1.quat[1] == vprn_data_2.quat[1] and + vprn_data_1.quat[2] == vprn_data_2.quat[2] and vprn_data_1.quat[3] == vprn_data_2.quat[3] and + vprn_data_1.pos[0] == vprn_data_2.pos[0] and vprn_data_1.pos[1] == vprn_data_2.pos[1] and + vprn_data_1.pos[2] == vprn_data_2.pos[2]); } // Tracker Position/Orientation Callback -void VRPN_CALLBACK track_target(void*, const vrpn_TRACKERCB tracker) { +void VRPN_CALLBACK track_target(void * nh_ptr, const vrpn_TRACKERCB tracker) +{ + auto nh = static_cast(nh_ptr); // Constructing the raw measured target pose variables. - Eigen::Quaterniond orientation_in(tracker.quat[3], tracker.quat[0], - tracker.quat[1], tracker.quat[2]); + Eigen::Quaterniond orientation_in( + tracker.quat[3], tracker.quat[0], tracker.quat[1], tracker.quat[2]); Eigen::Vector3d position_in(tracker.pos[0], tracker.pos[1], tracker.pos[2]); // Constructing the measured pose variables corrected for frame definitions Eigen::Quaterniond orientation_measured_B_W; Eigen::Vector3d position_measured_W; - correctForCoordinateSystem(orientation_in, position_in, coordinate_system, - &orientation_measured_B_W, &position_measured_W); + correctForCoordinateSystem( + nh, orientation_in, position_in, nh->coordinate_system, &orientation_measured_B_W, + &position_measured_W); // Verifying that each callback indeed gives fresh data. if (tracker_is_equal(prev_tracker, tracker)) { - ROS_WARN("Repeated Values"); + RCLCPP_WARN(nh->get_logger(), "Repeated Values"); } prev_tracker = tracker; // Timestamping the incomming message const int kMicroSecToNanoSec = 1000; - ros::Time tracker_timestamp(tracker.msg_time.tv_sec, - tracker.msg_time.tv_usec * kMicroSecToNanoSec); - ros::Time timestamp; - getTimeStamp(tracker_timestamp, ×tamp); + // Changed time source to RCL_ROS_TIME to avoid errors while calculating + // delay. + rclcpp::Time tracker_timestamp( + tracker.msg_time.tv_sec, tracker.msg_time.tv_usec * kMicroSecToNanoSec, RCL_ROS_TIME); + rclcpp::Time timestamp; + getTimeStamp(nh, tracker_timestamp, ×tamp); // Updating the estimates with the new measurements. - vicon_odometry_estimator->updateEstimate(position_measured_W, - orientation_measured_B_W, - timestamp); - Eigen::Vector3d position_estimate_W = - vicon_odometry_estimator->getEstimatedPosition(); - Eigen::Vector3d velocity_estimate_W = - vicon_odometry_estimator->getEstimatedVelocity(); - Eigen::Quaterniond orientation_estimate_B_W = - vicon_odometry_estimator->getEstimatedOrientation(); - Eigen::Vector3d rate_estimate_B = - vicon_odometry_estimator->getEstimatedAngularVelocity(); + vicon_odometry_estimator->updateEstimate( + position_measured_W, orientation_measured_B_W, timestamp); + Eigen::Vector3d position_estimate_W = vicon_odometry_estimator->getEstimatedPosition(); + Eigen::Vector3d velocity_estimate_W = vicon_odometry_estimator->getEstimatedVelocity(); + Eigen::Quaterniond orientation_estimate_B_W = vicon_odometry_estimator->getEstimatedOrientation(); + Eigen::Vector3d rate_estimate_B = vicon_odometry_estimator->getEstimatedAngularVelocity(); // Publishing the estimator intermediate results // TODO(millanea): This is really only useful for estimator debugging and // should be removed @@ -312,118 +378,71 @@ void VRPN_CALLBACK track_target(void*, const vrpn_TRACKERCB tracker) { // Rotating the estimated global frame velocity into the body frame. Eigen::Vector3d velocity_estimate_B = - orientation_estimate_B_W.toRotationMatrix().transpose() * - velocity_estimate_W; + orientation_estimate_B_W.toRotationMatrix().transpose() * velocity_estimate_W; // Populate the raw measured transform message. Published in main loop. target_state->measured_transform.header.stamp = timestamp; - target_state->measured_transform.header.frame_id = coordinate_system_string; - target_state->measured_transform.child_frame_id = object_name; - tf2::toMsg(position_measured_W, - target_state->measured_transform.transform.translation); + target_state->measured_transform.header.frame_id = nh->coordinate_system_string; + target_state->measured_transform.child_frame_id = nh->object_name; + tf2::toMsg(position_measured_W, target_state->measured_transform.transform.translation); target_state->measured_transform.transform.rotation = tf2::toMsg(orientation_measured_B_W); // Populate the estimated transform message. Published in main loop. target_state->estimated_transform.header.stamp = timestamp; - target_state->estimated_transform.header.frame_id = coordinate_system_string; - target_state->estimated_transform.child_frame_id = object_name; - tf2::toMsg(position_estimate_W, - target_state->estimated_transform.transform.translation); - target_state->estimated_transform.transform.rotation = tf2::toMsg( - orientation_estimate_B_W); + target_state->estimated_transform.header.frame_id = nh->coordinate_system_string; + target_state->estimated_transform.child_frame_id = nh->object_name; + tf2::toMsg(position_estimate_W, target_state->estimated_transform.transform.translation); + target_state->estimated_transform.transform.rotation = tf2::toMsg(orientation_estimate_B_W); // Populate the estimated odometry message. Published in main loop. target_state->estimated_odometry.header.stamp = timestamp; - target_state->estimated_odometry.header.frame_id = coordinate_system_string; - target_state->estimated_odometry.child_frame_id = object_name; + target_state->estimated_odometry.header.frame_id = nh->coordinate_system_string; + target_state->estimated_odometry.child_frame_id = nh->object_name; target_state->estimated_odometry.pose.pose.position = tf2::toMsg(position_estimate_W); - target_state->estimated_odometry.pose.pose.orientation = tf2::toMsg( - orientation_estimate_B_W); - tf2::toMsg(velocity_estimate_B, - target_state->estimated_odometry.twist.twist.linear); - tf2::toMsg(rate_estimate_B, - target_state->estimated_odometry.twist.twist.angular); + target_state->estimated_odometry.pose.pose.orientation = tf2::toMsg(orientation_estimate_B_W); + tf2::toMsg(velocity_estimate_B, target_state->estimated_odometry.twist.twist.linear); + tf2::toMsg(rate_estimate_B, target_state->estimated_odometry.twist.twist.angular); // Indicating to the main loop the data is ready for publishing. fresh_data = true; } -int main(int argc, char* argv[]) { - ros::init(argc, argv, "vrpn_client", ros::init_options::AnonymousName); - ros::NodeHandle nh(""); - ros::NodeHandle private_nh("~"); +int main(int argc, char * argv[]) +{ + rclcpp::init(argc, argv); - target_state = new TargetState; - - // Retrieving control parameters - std::string vrpn_server_ip; - int vrpn_port; - std::string trackedObjectName; - std::string timestamping_system_string; - private_nh.param("vrpn_server_ip", vrpn_server_ip, - std::string()); - private_nh.param("vrpn_port", vrpn_port, 3883); - private_nh.param("vrpn_coordinate_system", - coordinate_system_string, "vicon"); - private_nh.param("object_name", object_name, "auk"); - private_nh.param("timestamping_system", - timestamping_system_string, "tracker"); - private_nh.param("display_time_delay", display_time_delay, true); - - // Debug output - std::cout << "vrpn_server_ip:" << vrpn_server_ip << std::endl; - std::cout << "vrpn_port:" << vrpn_port << std::endl; - std::cout << "vrpn_coordinate_system:" << coordinate_system_string - << std::endl; - std::cout << "object_name:" << object_name << std::endl; - std::cout << "timestamping_system:" << timestamping_system_string - << std::endl; - - // Setting the coordinate system based on the ros param - if (coordinate_system_string == "vicon") { - coordinate_system = CoordinateSystem::kVicon; - } else if (coordinate_system_string == "optitrack") { - coordinate_system = CoordinateSystem::kOptitrack; - } else { - ROS_FATAL( - "ROS param vrpn_coordinate_system should be either 'vicon' or " - "'optitrack'!"); - return EXIT_FAILURE; - } + rclcpp::NodeOptions options; + rclcpp::executors::SingleThreadedExecutor exec; - // Setting the time stamping option based on the ros param - if (timestamping_system_string == "tracker") { - timestamping_system = TimestampingSystem::kTrackerStamp; - } else if (timestamping_system_string == "ros") { - timestamping_system = TimestampingSystem::kRosStamp; - } else { - ROS_FATAL( - "ROS param timestamping_system should be either 'tracker' or 'ros'!"); - return EXIT_FAILURE; - } + target_state = new TargetState; // Creating the estimator - vicon_odometry_estimator = - new vicon_estimator::ViconOdometryEstimator(private_nh); - vicon_odometry_estimator->initializeParameters(private_nh); + vicon_odometry_estimator = std::make_shared(options); + vicon_odometry_estimator->initializeParameters(); vicon_odometry_estimator->reset(); // Creating object which handles data publishing - Rigid_Body tool(private_nh, vrpn_server_ip, vrpn_port, object_name); + auto tool = std::make_shared(options); + + exec.add_node(vicon_odometry_estimator); + exec.add_node(tool); - ros::Rate loop_rate(1000); // TODO(gohlp): fix this + rclcpp::Rate loop_rate(1000); // TODO(gohlp): fix this - while (ros::ok()) { - tool.step_vrpn(); + while (rclcpp::ok()) { + tool->step_vrpn(); // Publishing newly received data. if (fresh_data == true) { - tool.publish_measured_transform(target_state); - tool.publish_estimated_transform(target_state); - tool.publish_estimated_odometry(target_state); + tool->publish_measured_transform(target_state); + tool->publish_estimated_transform(target_state); + tool->publish_estimated_odometry(target_state); fresh_data = false; } loop_rate.sleep(); } + + rclcpp::shutdown(); + return 0; } diff --git a/src/test/include/test_helper_library.h b/src/test/include/test_helper_library.h index bf11aad..de54f3a 100644 --- a/src/test/include/test_helper_library.h +++ b/src/test/include/test_helper_library.h @@ -26,14 +26,14 @@ #include -void euler2quat(double roll, double pitch, double yaw, double* q); +void euler2quat(double roll, double pitch, double yaw, double * q); -void calculate3dRmsError(double truth[][3], double est[][3], - const int trajectory_length, const int start_index, - double* error); +void calculate3dRmsError( + double truth[][3], double est[][3], const int trajectory_length, const int start_index, + double * error); -void calculateQuaternionRmsError(double truth[][4], double est[][4], - const int trajectory_length, - const int start_index, double* error); +void calculateQuaternionRmsError( + double truth[][4], double est[][4], const int trajectory_length, const int start_index, + double * error); #endif // TEST_HELPER_LIBRARY_H diff --git a/src/test/library/test_helper_library.cpp b/src/test/library/test_helper_library.cpp index 33322fe..4a3ff1d 100644 --- a/src/test/library/test_helper_library.cpp +++ b/src/test/library/test_helper_library.cpp @@ -2,7 +2,8 @@ #include "test_helper_library.h" -void euler2quat(double roll, double pitch, double yaw, double* q) { +void euler2quat(double roll, double pitch, double yaw, double * q) +{ // Compute quaternion Eigen::Quaterniond q_ = Eigen::AngleAxisd(yaw, Eigen::Vector3d::UnitZ()) * Eigen::AngleAxisd(pitch, Eigen::Vector3d::UnitY()) * @@ -14,9 +15,10 @@ void euler2quat(double roll, double pitch, double yaw, double* q) { q[3] = q_.z(); } -void calculate3dRmsError(double truth[][3], double est[][3], - const int trajectory_length, const int start_index, - double* error) { +void calculate3dRmsError( + double truth[][3], double est[][3], const int trajectory_length, const int start_index, + double * error) +{ // Looping over trajectories summing squared errors double error_sum[3] = {0.0, 0.0, 0.0}; for (int i = start_index; i < trajectory_length; i++) { @@ -35,20 +37,18 @@ void calculate3dRmsError(double truth[][3], double est[][3], error[2] = sqrt(error_sum[2]); } -void calculateQuaternionRmsError(double truth[][4], double est[][4], - const int trajectory_length, - const int start_index, double* error) { +void calculateQuaternionRmsError( + double truth[][4], double est[][4], const int trajectory_length, const int start_index, + double * error) +{ // Generating the error trajectory double error_trajectory[trajectory_length][3]; for (int i = 0; i < trajectory_length; i++) { // Turning vectors into quaternions - Eigen::Quaterniond orientation_truth(truth[i][0], truth[i][1], truth[i][2], - truth[i][3]); - Eigen::Quaterniond orientation_estimate(est[i][0], est[i][1], est[i][2], - est[i][3]); + Eigen::Quaterniond orientation_truth(truth[i][0], truth[i][1], truth[i][2], truth[i][3]); + Eigen::Quaterniond orientation_estimate(est[i][0], est[i][1], est[i][2], est[i][3]); // Calculating the error quaternion - Eigen::Quaterniond error_quaternion = - orientation_estimate.inverse() * orientation_truth; + Eigen::Quaterniond error_quaternion = orientation_estimate.inverse() * orientation_truth; // Extracting the three meaningful components of the error quaternion error_trajectory[i][0] = error_quaternion.x(); error_trajectory[i][1] = error_quaternion.y(); diff --git a/src/test/test_vicon_estimator.cpp b/src/test/test_vicon_estimator.cpp index 1d4b548..50db549 100644 --- a/src/test/test_vicon_estimator.cpp +++ b/src/test/test_vicon_estimator.cpp @@ -19,12 +19,12 @@ * limitations under the License. */ +#include #include -#include -#include -#include #include +#include +#include #include "test_helper_library.h" #include "vicon_estimator.h" @@ -73,39 +73,39 @@ double* q3, double* q4); * Translational Estimator Tests */ -void generateTranslationalTrajectorySinusoidal(const int trajectory_length, - double position_trajectory[][3], - double velocity_trajectory[][3], - double timestamps[]) { +void generateTranslationalTrajectorySinusoidal( + const int trajectory_length, double position_trajectory[][3], double velocity_trajectory[][3], + double timestamps[]) +{ // Generating position trajectories for (int i = 0; i < trajectory_length; i++) { position_trajectory[i][0] = - TRANS_TRAJECTORY_AMPLITUDE * - sin(2 * M_PI * i * TRANS_TRAJECTORY_DT * TRANS_TRAJECTORY_FREQ + - TRANS_TRAJECTORY_PHASE_OFFSET_X); + TRANS_TRAJECTORY_AMPLITUDE * sin( + 2 * M_PI * i * TRANS_TRAJECTORY_DT * TRANS_TRAJECTORY_FREQ + + TRANS_TRAJECTORY_PHASE_OFFSET_X); position_trajectory[i][1] = - TRANS_TRAJECTORY_AMPLITUDE * - sin(2 * M_PI * i * TRANS_TRAJECTORY_DT * TRANS_TRAJECTORY_FREQ + - TRANS_TRAJECTORY_PHASE_OFFSET_Y); + TRANS_TRAJECTORY_AMPLITUDE * sin( + 2 * M_PI * i * TRANS_TRAJECTORY_DT * TRANS_TRAJECTORY_FREQ + + TRANS_TRAJECTORY_PHASE_OFFSET_Y); position_trajectory[i][2] = - TRANS_TRAJECTORY_AMPLITUDE * - sin(2 * M_PI * i * TRANS_TRAJECTORY_DT * TRANS_TRAJECTORY_FREQ + - TRANS_TRAJECTORY_PHASE_OFFSET_Z); + TRANS_TRAJECTORY_AMPLITUDE * sin( + 2 * M_PI * i * TRANS_TRAJECTORY_DT * TRANS_TRAJECTORY_FREQ + + TRANS_TRAJECTORY_PHASE_OFFSET_Z); } // Generating velocity trajectories from algebraic differentiation for (int i = 0; i < trajectory_length; i++) { - velocity_trajectory[i][0] = - 2 * M_PI * TRANS_TRAJECTORY_FREQ * - cos(2 * M_PI * i * TRANS_TRAJECTORY_DT * TRANS_TRAJECTORY_FREQ + - TRANS_TRAJECTORY_PHASE_OFFSET_X); - velocity_trajectory[i][1] = - 2 * M_PI * TRANS_TRAJECTORY_FREQ * - cos(2 * M_PI * i * TRANS_TRAJECTORY_DT * TRANS_TRAJECTORY_FREQ + - TRANS_TRAJECTORY_PHASE_OFFSET_Y); - velocity_trajectory[i][2] = - 2 * M_PI * TRANS_TRAJECTORY_FREQ * - cos(2 * M_PI * i * TRANS_TRAJECTORY_DT * TRANS_TRAJECTORY_FREQ + - TRANS_TRAJECTORY_PHASE_OFFSET_Z); + velocity_trajectory[i][0] = 2 * M_PI * TRANS_TRAJECTORY_FREQ * + cos( + 2 * M_PI * i * TRANS_TRAJECTORY_DT * TRANS_TRAJECTORY_FREQ + + TRANS_TRAJECTORY_PHASE_OFFSET_X); + velocity_trajectory[i][1] = 2 * M_PI * TRANS_TRAJECTORY_FREQ * + cos( + 2 * M_PI * i * TRANS_TRAJECTORY_DT * TRANS_TRAJECTORY_FREQ + + TRANS_TRAJECTORY_PHASE_OFFSET_Y); + velocity_trajectory[i][2] = 2 * M_PI * TRANS_TRAJECTORY_FREQ * + cos( + 2 * M_PI * i * TRANS_TRAJECTORY_DT * TRANS_TRAJECTORY_FREQ + + TRANS_TRAJECTORY_PHASE_OFFSET_Z); } // Generating the timestamps timestamps[0] = 0.0; @@ -114,42 +114,39 @@ void generateTranslationalTrajectorySinusoidal(const int trajectory_length, } } -TEST(translationalEstimator, sinusoidal_clean) { +TEST(translationalEstimator, sinusoidal_clean) +{ // Creating the estimator vicon_estimator::TranslationalEstimator translational_estimator; // Setting the estimator gains - vicon_estimator::TranslationalEstimatorParameters - translational_estimator_parameters; + vicon_estimator::TranslationalEstimatorParameters translational_estimator_parameters; translational_estimator_parameters.kp_ = 1.0; translational_estimator_parameters.kv_ = 10 * 10.0; translational_estimator.setParameters(translational_estimator_parameters); translational_estimator.reset(); // Generating the trajectory over which to test the estimator - const int trajectory_length = - static_cast(TRANS_TRAJECTORY_PERIOD / TRANS_TRAJECTORY_DT) + 1; + const int trajectory_length = static_cast(TRANS_TRAJECTORY_PERIOD / TRANS_TRAJECTORY_DT) + 1; double position_trajectory[trajectory_length][3]; double velocity_trajectory[trajectory_length][3]; double timestamps[trajectory_length]; generateTranslationalTrajectorySinusoidal( - trajectory_length, position_trajectory, velocity_trajectory, timestamps); + trajectory_length, position_trajectory, velocity_trajectory, timestamps); // Looping over trajectory and retrieving estimates double position_trajectory_estimate[trajectory_length][3]; double velocity_trajectory_estimate[trajectory_length][3]; for (int i = 0; i < trajectory_length; i++) { // Constructing input - Eigen::Vector3d input(position_trajectory[i][0], position_trajectory[i][1], - position_trajectory[i][2]); + Eigen::Vector3d input( + position_trajectory[i][0], position_trajectory[i][1], position_trajectory[i][2]); double timestamp = timestamps[i]; // Updating the estimate with the measurement translational_estimator.updateEstimate(input, timestamp); // Getting the position and velocity estimates - Eigen::Vector3d estimated_position = - translational_estimator.getEstimatedPosition(); - Eigen::Vector3d estimated_velocity = - translational_estimator.getEstimatedVelocity(); + Eigen::Vector3d estimated_position = translational_estimator.getEstimatedPosition(); + Eigen::Vector3d estimated_velocity = translational_estimator.getEstimatedVelocity(); // Moving values to arrays position_trajectory_estimate[i][0] = estimated_position.x(); position_trajectory_estimate[i][1] = estimated_position.y(); @@ -164,29 +161,25 @@ TEST(translationalEstimator, sinusoidal_clean) { // Calculating position estimate errors double position_error[3]; - calculate3dRmsError(position_trajectory, position_trajectory_estimate, - trajectory_length, start_index, position_error); + calculate3dRmsError( + position_trajectory, position_trajectory_estimate, trajectory_length, start_index, + position_error); // Performing test - EXPECT_NEAR(position_error[0], 0, POS_ERROR_THRESHOLD) - << "X position estimate error too great"; - EXPECT_NEAR(position_error[1], 0, POS_ERROR_THRESHOLD) - << "Y position estimate error too great"; - EXPECT_NEAR(position_error[2], 0, POS_ERROR_THRESHOLD) - << "Z position estimate error too great"; + EXPECT_NEAR(position_error[0], 0, POS_ERROR_THRESHOLD) << "X position estimate error too great"; + EXPECT_NEAR(position_error[1], 0, POS_ERROR_THRESHOLD) << "Y position estimate error too great"; + EXPECT_NEAR(position_error[2], 0, POS_ERROR_THRESHOLD) << "Z position estimate error too great"; // Calculating velocity estimate errors double velocity_error[3]; - calculate3dRmsError(velocity_trajectory, velocity_trajectory_estimate, - trajectory_length, start_index, velocity_error); + calculate3dRmsError( + velocity_trajectory, velocity_trajectory_estimate, trajectory_length, start_index, + velocity_error); // Performing test - EXPECT_NEAR(velocity_error[0], 0, VEL_ERROR_THRESHOLD) - << "X velocity estimate error too great"; - EXPECT_NEAR(velocity_error[1], 0, VEL_ERROR_THRESHOLD) - << "Y velocity estimate error too great"; - EXPECT_NEAR(velocity_error[2], 0, VEL_ERROR_THRESHOLD) - << "Z velocity estimate error too great"; + EXPECT_NEAR(velocity_error[0], 0, VEL_ERROR_THRESHOLD) << "X velocity estimate error too great"; + EXPECT_NEAR(velocity_error[1], 0, VEL_ERROR_THRESHOLD) << "Y velocity estimate error too great"; + EXPECT_NEAR(velocity_error[2], 0, VEL_ERROR_THRESHOLD) << "Z velocity estimate error too great"; /* * For matlab debug @@ -227,23 +220,23 @@ TEST(translationalEstimator, sinusoidal_clean) { * Rotational Estimator Tests */ -void generateRotationalTrajectorySinusoidal(const int trajectory_length, - double orientation_trajectory[][4], - double rollrate_trajectory[][3], - double timestamps[]) { +void generateRotationalTrajectorySinusoidal( + const int trajectory_length, double orientation_trajectory[][4], double rollrate_trajectory[][3], + double timestamps[]) +{ // Generating quaternion trajectories double roll, pitch, yaw; for (int i = 0; i < trajectory_length; i++) { // Generating euler angles - roll = ROT_TRAJECTORY_AMPLITUDE * - sin(2 * M_PI * i * ROT_TRAJECTORY_DT * ROT_TRAJECTORY_FREQ + - ROT_TRAJECTORY_PHASE_OFFSET_X); - pitch = ROT_TRAJECTORY_AMPLITUDE * - sin(2 * M_PI * i * ROT_TRAJECTORY_DT * ROT_TRAJECTORY_FREQ + - ROT_TRAJECTORY_PHASE_OFFSET_Y); - yaw = ROT_TRAJECTORY_AMPLITUDE * - sin(2 * M_PI * i * ROT_TRAJECTORY_DT * ROT_TRAJECTORY_FREQ + - ROT_TRAJECTORY_PHASE_OFFSET_Z); + roll = + ROT_TRAJECTORY_AMPLITUDE * + sin(2 * M_PI * i * ROT_TRAJECTORY_DT * ROT_TRAJECTORY_FREQ + ROT_TRAJECTORY_PHASE_OFFSET_X); + pitch = + ROT_TRAJECTORY_AMPLITUDE * + sin(2 * M_PI * i * ROT_TRAJECTORY_DT * ROT_TRAJECTORY_FREQ + ROT_TRAJECTORY_PHASE_OFFSET_Y); + yaw = + ROT_TRAJECTORY_AMPLITUDE * + sin(2 * M_PI * i * ROT_TRAJECTORY_DT * ROT_TRAJECTORY_FREQ + ROT_TRAJECTORY_PHASE_OFFSET_Z); // Converting to quaternions euler2quat(roll, pitch, yaw, orientation_trajectory[i]); } @@ -253,16 +246,15 @@ void generateRotationalTrajectorySinusoidal(const int trajectory_length, for (int i = 0; i < trajectory_length - 1; i++) { // Generating quaternions q_k = Eigen::Quaterniond( - orientation_trajectory[i + 1][0], orientation_trajectory[i + 1][1], - orientation_trajectory[i + 1][2], orientation_trajectory[i + 1][3]); + orientation_trajectory[i + 1][0], orientation_trajectory[i + 1][1], + orientation_trajectory[i + 1][2], orientation_trajectory[i + 1][3]); q_k_1 = Eigen::Quaterniond( - orientation_trajectory[i][0], orientation_trajectory[i][1], - orientation_trajectory[i][2], orientation_trajectory[i][3]); + orientation_trajectory[i][0], orientation_trajectory[i][1], orientation_trajectory[i][2], + orientation_trajectory[i][3]); // Calculating quaternion derivative Eigen::Quaterniond diff = - Eigen::Quaterniond((q_k.coeffs() - q_k_1.coeffs()) / ROT_TRAJECTORY_DT); - Eigen::Quaterniond omega = - Eigen::Quaterniond(2 * (q_k_1.inverse() * diff).coeffs()); + Eigen::Quaterniond((q_k.coeffs() - q_k_1.coeffs()) / ROT_TRAJECTORY_DT); + Eigen::Quaterniond omega = Eigen::Quaterniond(2 * (q_k_1.inverse() * diff).coeffs()); // Writing to the trajectory rollrate_trajectory[i][0] = omega.x(); rollrate_trajectory[i][1] = omega.y(); @@ -276,13 +268,13 @@ void generateRotationalTrajectorySinusoidal(const int trajectory_length, } } -TEST(rotationalEstimator, sinusoidal_clean) { +TEST(rotationalEstimator, sinusoidal_clean) +{ // Creating the estimator vicon_estimator::RotationalEstimator rotational_estimator; // Setting the estimator gains - vicon_estimator::RotationalEstimatorParameters - rotational_estimator_parameters; + vicon_estimator::RotationalEstimatorParameters rotational_estimator_parameters; rotational_estimator_parameters.dorientation_estimate_initial_covariance_ = 1; rotational_estimator_parameters.drate_estimate_initial_covariance_ = 1; rotational_estimator_parameters.dorientation_process_covariance_ = 0.01; @@ -292,13 +284,12 @@ TEST(rotationalEstimator, sinusoidal_clean) { rotational_estimator.reset(); // Generating the trajectory over which to test the estimator - const int trajectory_length = - static_cast(ROT_TRAJECTORY_PERIOD / ROT_TRAJECTORY_DT) + 1; + const int trajectory_length = static_cast(ROT_TRAJECTORY_PERIOD / ROT_TRAJECTORY_DT) + 1; double orientation_trajectory[trajectory_length][4]; double omega_trajectory[trajectory_length][3]; double timestamps[trajectory_length]; generateRotationalTrajectorySinusoidal( - trajectory_length, orientation_trajectory, omega_trajectory, timestamps); + trajectory_length, orientation_trajectory, omega_trajectory, timestamps); // Looping over trajectory and retrieving estimates double orientation_estimate_trajectory[trajectory_length][4]; @@ -306,16 +297,14 @@ TEST(rotationalEstimator, sinusoidal_clean) { for (int i = 0; i < trajectory_length; i++) { // Constructing input Eigen::Quaterniond input( - orientation_trajectory[i][0], orientation_trajectory[i][1], - orientation_trajectory[i][2], orientation_trajectory[i][3]); + orientation_trajectory[i][0], orientation_trajectory[i][1], orientation_trajectory[i][2], + orientation_trajectory[i][3]); // Updating the estimate with the measurement double timestamp = timestamps[i]; rotational_estimator.updateEstimate(input, timestamp); // Getting the position and velocity estimates - Eigen::Quaterniond estimated_orientation = - rotational_estimator.getEstimatedOrientation(); - Eigen::Vector3d estimated_rollrate = - rotational_estimator.getEstimatedRate(); + Eigen::Quaterniond estimated_orientation = rotational_estimator.getEstimatedOrientation(); + Eigen::Vector3d estimated_rollrate = rotational_estimator.getEstimatedRate(); // Moving values to arrays orientation_estimate_trajectory[i][0] = estimated_orientation.w(); orientation_estimate_trajectory[i][1] = estimated_orientation.x(); @@ -332,29 +321,29 @@ TEST(rotationalEstimator, sinusoidal_clean) { // Calculating position estimate errors double orientation_error[3]; calculateQuaternionRmsError( - orientation_trajectory, orientation_estimate_trajectory, - trajectory_length, start_index, orientation_error); + orientation_trajectory, orientation_estimate_trajectory, trajectory_length, start_index, + orientation_error); // Performing test EXPECT_NEAR(orientation_error[0], 0, QUAT_ERROR_THRESHOLD) - << "X errorquaternion estimate error too great"; + << "X errorquaternion estimate error too great"; EXPECT_NEAR(orientation_error[1], 0, QUAT_ERROR_THRESHOLD) - << "Y errorquaternion estimate error too great"; + << "Y errorquaternion estimate error too great"; EXPECT_NEAR(orientation_error[2], 0, QUAT_ERROR_THRESHOLD) - << "Z errorquaternion estimate error too great"; + << "Z errorquaternion estimate error too great"; // Calculating velocity estimate errors double omegaError[3]; - calculate3dRmsError(omega_trajectory, rollrate_estimate_trajectory, - trajectory_length, start_index, omegaError); + calculate3dRmsError( + omega_trajectory, rollrate_estimate_trajectory, trajectory_length, start_index, omegaError); // Performing test EXPECT_NEAR(omegaError[0], 0, OMEGA_ERROR_THRESHOLD) - << "X rotational velocity estimate error too great"; + << "X rotational velocity estimate error too great"; EXPECT_NEAR(omegaError[1], 0, OMEGA_ERROR_THRESHOLD) - << "Y rotational velocity estimate error too great"; + << "Y rotational velocity estimate error too great"; EXPECT_NEAR(omegaError[2], 0, OMEGA_ERROR_THRESHOLD) - << "Z rotational velocity estimate error too great"; + << "Z rotational velocity estimate error too great"; /* * For matlab debug @@ -395,48 +384,44 @@ TEST(rotationalEstimator, sinusoidal_clean) { // Quaternions representing flips in various dimensions const int kNumberOfInversions = 7; const Eigen::Quaterniond rotation_inversion_x = - Eigen::Quaterniond(Eigen::AngleAxisd(M_PI, Eigen::Vector3d::UnitX())); + Eigen::Quaterniond(Eigen::AngleAxisd(M_PI, Eigen::Vector3d::UnitX())); const Eigen::Quaterniond rotation_inversion_y = - Eigen::Quaterniond(Eigen::AngleAxisd(M_PI, Eigen::Vector3d::UnitY())); + Eigen::Quaterniond(Eigen::AngleAxisd(M_PI, Eigen::Vector3d::UnitY())); const Eigen::Quaterniond rotation_inversion_z = - Eigen::Quaterniond(Eigen::AngleAxisd(M_PI, Eigen::Vector3d::UnitZ())); -const Eigen::Quaterniond rotation_inversion_xy = - rotation_inversion_x * rotation_inversion_y; -const Eigen::Quaterniond rotation_inversion_yz = - rotation_inversion_y * rotation_inversion_z; -const Eigen::Quaterniond rotation_inversion_xz = - rotation_inversion_x * rotation_inversion_z; + Eigen::Quaterniond(Eigen::AngleAxisd(M_PI, Eigen::Vector3d::UnitZ())); +const Eigen::Quaterniond rotation_inversion_xy = rotation_inversion_x * rotation_inversion_y; +const Eigen::Quaterniond rotation_inversion_yz = rotation_inversion_y * rotation_inversion_z; +const Eigen::Quaterniond rotation_inversion_xz = rotation_inversion_x * rotation_inversion_z; const Eigen::Quaterniond rotation_inversion_xyz = - rotation_inversion_x * rotation_inversion_y * rotation_inversion_z; + rotation_inversion_x * rotation_inversion_y * rotation_inversion_z; const Eigen::Quaterniond rotation_inversions[kNumberOfInversions] = { - rotation_inversion_x, rotation_inversion_y, rotation_inversion_z, - rotation_inversion_xy, rotation_inversion_yz, rotation_inversion_xz, - rotation_inversion_xyz}; + rotation_inversion_x, rotation_inversion_y, rotation_inversion_z, rotation_inversion_xy, + rotation_inversion_yz, rotation_inversion_xz, rotation_inversion_xyz}; void generateCorruptedInputTrajectory( - const Eigen::Quaterniond clean_trajectory[], const int trajectory_length, - const int corruption_rate, Eigen::Quaterniond corrupted_trajectory[]) { + const Eigen::Quaterniond clean_trajectory[], const int trajectory_length, + const int corruption_rate, Eigen::Quaterniond corrupted_trajectory[]) +{ // Looping over the clean input trajecty and corruputing measurements at a // rate determined by corruption_rate // Note that corruption starts only after corruption_rate samples. int inversion_index = 0; for (int i = corruption_rate; i <= trajectory_length; i += corruption_rate) { // Corrupting the measurement with an inversion from the inversion list - corrupted_trajectory[i] = - rotation_inversions[inversion_index] * clean_trajectory[i]; + corrupted_trajectory[i] = rotation_inversions[inversion_index] * clean_trajectory[i]; // corrupted_trajectory[i] = clean_trajectory[i]; // Incrementing the inversion inversion_index = (inversion_index + 1) % kNumberOfInversions; } } -TEST(rotationalEstimator, sinusoidal_corrupted) { +TEST(rotationalEstimator, sinusoidal_corrupted) +{ // Creating the estimator vicon_estimator::RotationalEstimator rotational_estimator; // Setting the estimator gains - vicon_estimator::RotationalEstimatorParameters - rotational_estimator_parameters; + vicon_estimator::RotationalEstimatorParameters rotational_estimator_parameters; rotational_estimator_parameters.dorientation_estimate_initial_covariance_ = 1; rotational_estimator_parameters.drate_estimate_initial_covariance_ = 1; rotational_estimator_parameters.dorientation_process_covariance_ = 0.01; @@ -446,13 +431,12 @@ TEST(rotationalEstimator, sinusoidal_corrupted) { rotational_estimator.reset(); // Generating the trajectory over which to test the estimator - const int trajectory_length = - static_cast(ROT_TRAJECTORY_PERIOD / ROT_TRAJECTORY_DT) + 1; + const int trajectory_length = static_cast(ROT_TRAJECTORY_PERIOD / ROT_TRAJECTORY_DT) + 1; double orientation_trajectory[trajectory_length][4]; double omega_trajectory[trajectory_length][3]; double timestamps[trajectory_length]; generateRotationalTrajectorySinusoidal( - trajectory_length, orientation_trajectory, omega_trajectory, timestamps); + trajectory_length, orientation_trajectory, omega_trajectory, timestamps); // Constructing the measurement vector Eigen::Quaterniond clean_input_trajectory[trajectory_length]; @@ -460,29 +444,26 @@ TEST(rotationalEstimator, sinusoidal_corrupted) { // Creating the clean measurement vector for (int i = 0; i < trajectory_length; i++) { clean_input_trajectory[i] = Eigen::Quaterniond( - orientation_trajectory[i][0], orientation_trajectory[i][1], - orientation_trajectory[i][2], orientation_trajectory[i][3]); + orientation_trajectory[i][0], orientation_trajectory[i][1], orientation_trajectory[i][2], + orientation_trajectory[i][3]); corrupted_input_trajectory[i] = Eigen::Quaterniond( - orientation_trajectory[i][0], orientation_trajectory[i][1], - orientation_trajectory[i][2], orientation_trajectory[i][3]); + orientation_trajectory[i][0], orientation_trajectory[i][1], orientation_trajectory[i][2], + orientation_trajectory[i][3]); } // Corrupting the measurement vector int corruption_rate = 100; // TODO(millanea): Parameter above. - generateCorruptedInputTrajectory(clean_input_trajectory, trajectory_length, - corruption_rate, corrupted_input_trajectory); + generateCorruptedInputTrajectory( + clean_input_trajectory, trajectory_length, corruption_rate, corrupted_input_trajectory); // Looping over trajectory and retrieving estimates double orientation_estimate_trajectory[trajectory_length][4]; double rollrate_estimate_trajectory[trajectory_length][3]; for (int i = 0; i < trajectory_length; i++) { // Updating the estimate with the measurement - rotational_estimator.updateEstimate(corrupted_input_trajectory[i], - timestamps[i]); + rotational_estimator.updateEstimate(corrupted_input_trajectory[i], timestamps[i]); // Getting the position and velocity estimates - Eigen::Quaterniond estimated_orientation = - rotational_estimator.getEstimatedOrientation(); - Eigen::Vector3d estimated_rollrate = - rotational_estimator.getEstimatedRate(); + Eigen::Quaterniond estimated_orientation = rotational_estimator.getEstimatedOrientation(); + Eigen::Vector3d estimated_rollrate = rotational_estimator.getEstimatedRate(); // Moving values to arrays orientation_estimate_trajectory[i][0] = estimated_orientation.w(); orientation_estimate_trajectory[i][1] = estimated_orientation.x(); @@ -499,36 +480,37 @@ TEST(rotationalEstimator, sinusoidal_corrupted) { // Calculating position estimate errors double orientation_error[3]; calculateQuaternionRmsError( - orientation_trajectory, orientation_estimate_trajectory, - trajectory_length, start_index, orientation_error); + orientation_trajectory, orientation_estimate_trajectory, trajectory_length, start_index, + orientation_error); // Performing test EXPECT_NEAR(orientation_error[0], 0, QUAT_ERROR_THRESHOLD) - << "X errorquaternion estimate error too great"; + << "X errorquaternion estimate error too great"; EXPECT_NEAR(orientation_error[1], 0, QUAT_ERROR_THRESHOLD) - << "Y errorquaternion estimate error too great"; + << "Y errorquaternion estimate error too great"; EXPECT_NEAR(orientation_error[2], 0, QUAT_ERROR_THRESHOLD) - << "Z errorquaternion estimate error too great"; + << "Z errorquaternion estimate error too great"; // Calculating velocity estimate errors double omegaError[3]; - calculate3dRmsError(omega_trajectory, rollrate_estimate_trajectory, - trajectory_length, start_index, omegaError); + calculate3dRmsError( + omega_trajectory, rollrate_estimate_trajectory, trajectory_length, start_index, omegaError); // Performing test EXPECT_NEAR(omegaError[0], 0, OMEGA_ERROR_THRESHOLD) - << "X rotational velocity estimate error too great"; + << "X rotational velocity estimate error too great"; EXPECT_NEAR(omegaError[1], 0, OMEGA_ERROR_THRESHOLD) - << "Y rotational velocity estimate error too great"; + << "Y rotational velocity estimate error too great"; EXPECT_NEAR(omegaError[2], 0, OMEGA_ERROR_THRESHOLD) - << "Z rotational velocity estimate error too great"; + << "Z rotational velocity estimate error too great"; } /* * GTests Main */ -int main(int argc, char **argv) { +int main(int argc, char ** argv) +{ testing::InitGoogleTest(&argc, argv); return RUN_ALL_TESTS(); } diff --git a/src/vicon_estimation_node.cpp b/src/vicon_estimation_node.cpp index 45f8f6c..df01cc4 100644 --- a/src/vicon_estimation_node.cpp +++ b/src/vicon_estimation_node.cpp @@ -24,120 +24,118 @@ // finally publishes position, velcocity, orientation and angular // velocity estimates. -#include -#include +#include +#include +#include +#include -#include -#include -#include #include -#include +#include +#include + +#include "vicon_odometry_estimator.hpp" -#include "vicon_odometry_estimator.h" +using std::placeholders::_1; // Class for collecting data and passing it to the underlying estimator -class ViconDataListener { - public: +class ViconDataListener : public rclcpp::Node +{ +public: // Constructor ViconDataListener( - ros::NodeHandle nh, ros::NodeHandle nh_private, - vicon_estimator::ViconOdometryEstimator* vicon_odometry_estimator) - : vicon_odometry_estimator_(vicon_odometry_estimator) { + const rclcpp::NodeOptions & options, + const std::shared_ptr vicon_odometry_estimator) + : Node("vicon_data_listener" , options), vicon_odometry_estimator_(vicon_odometry_estimator) + { + // Getting the object name + this->declare_parameter("object_name", "auk"); + this->get_parameter("object_name", object_name_); // Subscribing to the raw vicon data - raw_transform_sub_ = - nh.subscribe("vrpn_client/raw_transform", 10, - &ViconDataListener::transformStampedCallback, this); + raw_transform_sub_ = this->create_subscription( + "vrpn_client/raw_transform", 10, std::bind(&ViconDataListener::transformStampedCallback, this, _1)); // Advertising the estimated target state components estimated_transform_pub_ = - nh_private.advertise( - "estimated_transform", 10); - estimated_odometry_pub_ = - nh_private.advertise("estimated_odometry", 10); - // Getting the object name - nh_private.param("object_name", object_name_, "auk"); + create_publisher("estimated_transform", 10); + estimated_odometry_pub_ = create_publisher("estimated_odometry", 10); } // Raw vicon data callback. - void transformStampedCallback( - const geometry_msgs::TransformStampedConstPtr& msg) { + void transformStampedCallback(const geometry_msgs::msg::TransformStamped::SharedPtr msg) + { // Extracting the relavent data from the message Eigen::Vector3d position_measured_W; Eigen::Quaterniond orientation_measured_B_W; tf2::fromMsg(msg->transform.translation, position_measured_W); tf2::fromMsg(msg->transform.rotation, orientation_measured_B_W); - ros::Time timestamp = msg->header.stamp; + rclcpp::Time timestamp = msg->header.stamp; // Passing the received data to the estimator - vicon_odometry_estimator_->updateEstimate(position_measured_W, - orientation_measured_B_W, - timestamp); + vicon_odometry_estimator_->updateEstimate( + position_measured_W, orientation_measured_B_W, timestamp); // Retreiving the estimates - Eigen::Vector3d position_estimate_W = - vicon_odometry_estimator_->getEstimatedPosition(); - Eigen::Vector3d velocity_estimate_W = - vicon_odometry_estimator_->getEstimatedVelocity(); + Eigen::Vector3d position_estimate_W = vicon_odometry_estimator_->getEstimatedPosition(); + Eigen::Vector3d velocity_estimate_W = vicon_odometry_estimator_->getEstimatedVelocity(); Eigen::Quaterniond orientation_estimate_B_W = - vicon_odometry_estimator_->getEstimatedOrientation(); - Eigen::Vector3d rate_estimate_B = - vicon_odometry_estimator_->getEstimatedAngularVelocity(); + vicon_odometry_estimator_->getEstimatedOrientation(); + Eigen::Vector3d rate_estimate_B = vicon_odometry_estimator_->getEstimatedAngularVelocity(); // Rotating the estimated global frame velocity into the body frame. Eigen::Vector3d velocity_estimate_B = - orientation_estimate_B_W.toRotationMatrix() * velocity_estimate_W; + orientation_estimate_B_W.toRotationMatrix() * velocity_estimate_W; // Creating estimated transform message - geometry_msgs::TransformStamped estimated_transform; + geometry_msgs::msg::TransformStamped estimated_transform; estimated_transform.header = msg->header; - tf2::toMsg(position_estimate_W, - estimated_transform.transform.translation); + tf2::toMsg(position_estimate_W, estimated_transform.transform.translation); estimated_transform.transform.rotation = tf2::toMsg(orientation_estimate_B_W); // Creating estimated odometry message - nav_msgs::Odometry estimated_odometry; + nav_msgs::msg::Odometry estimated_odometry; estimated_odometry.header = msg->header; estimated_odometry.child_frame_id = object_name_; estimated_odometry.pose.pose.position = tf2::toMsg(position_estimate_W); estimated_odometry.pose.pose.orientation = tf2::toMsg(orientation_estimate_B_W); - tf2::toMsg(velocity_estimate_B, - estimated_odometry.twist.twist.linear); - tf2::toMsg(rate_estimate_B, - estimated_odometry.twist.twist.angular); + tf2::toMsg(velocity_estimate_B, estimated_odometry.twist.twist.linear); + tf2::toMsg(rate_estimate_B, estimated_odometry.twist.twist.angular); // Publishing the estimates - estimated_transform_pub_.publish(estimated_transform); - estimated_odometry_pub_.publish(estimated_odometry); + estimated_transform_pub_->publish(estimated_transform); + estimated_odometry_pub_->publish(estimated_odometry); // Publishing the estimator intermediate results vicon_odometry_estimator_->publishIntermediateResults(msg->header.stamp); } - private: +private: // Raw vicon data subscriber. - ros::Subscriber raw_transform_sub_; + rclcpp::Subscription::SharedPtr raw_transform_sub_; // Estimate publishers - ros::Publisher estimated_transform_pub_; - ros::Publisher estimated_odometry_pub_; + rclcpp::Publisher::SharedPtr estimated_transform_pub_; + rclcpp::Publisher::SharedPtr estimated_odometry_pub_; // Name of the tracked object std::string object_name_; // Vicon-based estimator - std::unique_ptr - vicon_odometry_estimator_; + std::shared_ptr vicon_odometry_estimator_; }; // Standard C++ entry point -int main(int argc, char** argv) { +int main(int argc, char ** argv) +{ // Announce this program to the ROS master - ros::init(argc, argv, "vicon_estimator"); - - // Creating the node handles - ros::NodeHandle nh; - ros::NodeHandle nh_private("~"); + rclcpp::init(argc, argv); + rclcpp::NodeOptions options; // Creating a Vicon-based estimator to do the estimation - vicon_estimator::ViconOdometryEstimator vicon_odometry_estimator(nh_private); - vicon_odometry_estimator.initializeParameters(nh_private); - vicon_odometry_estimator.reset(); + auto vicon_odometry_estimator = std::make_shared(options); + + vicon_odometry_estimator->initializeParameters(); + vicon_odometry_estimator->reset(); // Creating a Vicon Data Listener to direct vicon data to the estimator - ViconDataListener vicon_data_listener(nh, nh_private, - &vicon_odometry_estimator); + auto vicon_data_listener = std::make_shared(options, vicon_odometry_estimator); // Spinng forever pumping callbacks - ros::spin(); + rclcpp::executors::SingleThreadedExecutor exec; + exec.add_node(vicon_odometry_estimator); + exec.add_node(vicon_data_listener); + + exec.spin(); + + rclcpp::shutdown(); // Exit tranquilly return 0; } diff --git a/src/wrapper/vicon_odometry_estimator.cpp b/src/wrapper/vicon_odometry_estimator.cpp index bb3ab98..3ee62d6 100644 --- a/src/wrapper/vicon_odometry_estimator.cpp +++ b/src/wrapper/vicon_odometry_estimator.cpp @@ -19,120 +19,132 @@ * limitations under the License. */ -#include +#include "vicon_odometry_estimator.hpp" -#include "vicon_odometry_estimator.h" +#include -namespace vicon_estimator { +namespace vicon_estimator +{ -ViconOdometryEstimator::ViconOdometryEstimator(ros::NodeHandle& nh) - : vicon_estimator_(), verbose_(kDefaultVerboseFlag) { +ViconOdometryEstimator::ViconOdometryEstimator(const rclcpp::NodeOptions & options, const std::string & node_name) +: Node(node_name, options), vicon_estimator_(), verbose_(kDefaultVerboseFlag) +{ // Creating publisher for intermediate estimator values - publisher_ = nh.advertise( - "vicon_intermediate_results", 100); + publisher_ = create_publisher("vicon_intermediate_results", 100); } -void ViconOdometryEstimator::initializeParameters(ros::NodeHandle& nh) { +void ViconOdometryEstimator::initializeParameters() +{ // Recovering translational estimator parameters values from the parameter // server - vicon_estimator::TranslationalEstimatorParameters - translationalEstimatorParameters; - nh.getParam("translational_estimator/kp", - translationalEstimatorParameters.kp_); - nh.getParam("translational_estimator/kv", - translationalEstimatorParameters.kv_); + vicon_estimator::TranslationalEstimatorParameters translationalEstimatorParameters; + declare_parameter("translational_estimator/kp", 0.5); + get_parameter("translational_estimator/kp", translationalEstimatorParameters.kp_); + declare_parameter("translational_estimator/kv", 0.5); + get_parameter("translational_estimator/kv", translationalEstimatorParameters.kv_); // Recovering rotational estimator parameters values from the parameter server vicon_estimator::RotationalEstimatorParameters rotationalEstimatorParameters; - nh.getParam( - "rotational_estimator/orientation_estimate_initial_covariance", - rotationalEstimatorParameters.dorientation_estimate_initial_covariance_); - nh.getParam("rotational_estimator/rate_estimate_initial_covariance", - rotationalEstimatorParameters.drate_estimate_initial_covariance_); - nh.getParam("rotational_estimator/orientation_process_covariance", - rotationalEstimatorParameters.dorientation_process_covariance_); - nh.getParam("rotational_estimator/rate_process_covariance", - rotationalEstimatorParameters.drate_process_covariance_); - nh.getParam( - "rotational_estimator/orientation_measurementCovariance", - rotationalEstimatorParameters.orientation_measurement_covariance_); + declare_parameter("rotational_estimator/orientation_estimate_initial_covariance", 100.0); + get_parameter( + "rotational_estimator/orientation_estimate_initial_covariance", + rotationalEstimatorParameters.dorientation_estimate_initial_covariance_); + declare_parameter("rotational_estimator/rate_estimate_initial_covariance", 100.0); + get_parameter( + "rotational_estimator/rate_estimate_initial_covariance", + rotationalEstimatorParameters.drate_estimate_initial_covariance_); + declare_parameter("rotational_estimator/orientation_process_covariance", 0.000002); + get_parameter( + "rotational_estimator/orientation_process_covariance", + rotationalEstimatorParameters.dorientation_process_covariance_); + declare_parameter("rotational_estimator/rate_process_covariance", 10.0); + get_parameter( + "rotational_estimator/rate_process_covariance", + rotationalEstimatorParameters.drate_process_covariance_); + declare_parameter("rotational_estimator/orientation_measurementCovariance", 0.001); + get_parameter( + "rotational_estimator/orientation_measurementCovariance", + rotationalEstimatorParameters.orientation_measurement_covariance_); // Getting outlier rejection type std::string outlier_rejection_method_string; - nh.getParam("rotational_estimator/outlier_rejection_method", - outlier_rejection_method_string); + declare_parameter( + "rotational_estimator/outlier_rejection_method", "mahalanobis_distance"); + get_parameter( + "rotational_estimator/outlier_rejection_method", outlier_rejection_method_string); if (!outlier_rejection_method_string.compare("mahalanobis_distance")) { rotationalEstimatorParameters.outlier_rejection_method_ = - OutlierRejectionMethod::MAHALANOBIS_DISTANCE; - } else if (!outlier_rejection_method_string.compare( - "subsequent_measurements")) { + OutlierRejectionMethod::MAHALANOBIS_DISTANCE; + } else if (!outlier_rejection_method_string.compare("subsequent_measurements")) { rotationalEstimatorParameters.outlier_rejection_method_ = - OutlierRejectionMethod::SUBSEQUENT_MEASUREMENTS; + OutlierRejectionMethod::SUBSEQUENT_MEASUREMENTS; } else if (!outlier_rejection_method_string.compare("none")) { - rotationalEstimatorParameters.outlier_rejection_method_ = - OutlierRejectionMethod::NONE; + rotationalEstimatorParameters.outlier_rejection_method_ = OutlierRejectionMethod::NONE; } else { - ROS_WARN_STREAM( - "Outlier rejection method requested (" - << outlier_rejection_method_string - << ") Not recognized. Please select one of " - "\"mahalanobis_distance\", \"subsequent_measurements\", \"none\". " - "Using default value."); + RCLCPP_WARN_STREAM( + this->get_logger(), "Outlier rejection method requested (" + << outlier_rejection_method_string + << ") Not recognized. Please select " + "one of " + "\"mahalanobis_distance\", " + "\"subsequent_measurements\", " + "\"none\". " + "Using default value."); } - nh.getParam( - "rotational_estimator/outlier_rejection_mahalanobis_threshold", - rotationalEstimatorParameters.outlier_rejection_mahalanobis_threshold_); - - nh.getParam( - "rotational_estimator/outlier_rejection_subsequent_threshold_degrees", - rotationalEstimatorParameters - .outlier_rejection_subsequent_threshold_degrees_); - nh.getParam("rotational_estimator/outlier_rejection_subsequent_maximum_count", - rotationalEstimatorParameters - .outlier_rejection_subsequent_maximum_count_); - - nh.getParam("rotational_estimator/output_minimal_quaternions", - rotationalEstimatorParameters.output_minimal_quaternions_); - - nh.getParam("verbose", verbose_); + declare_parameter("rotational_estimator/outlier_rejection_mahalanobis_threshold", 4.0); + get_parameter( + "rotational_estimator/outlier_rejection_mahalanobis_threshold", + rotationalEstimatorParameters.outlier_rejection_mahalanobis_threshold_); + + declare_parameter( + "rotational_estimator/outlier_rejection_subsequent_threshold_degrees", 30.0); + get_parameter( + "rotational_estimator/outlier_rejection_subsequent_threshold_degrees", + rotationalEstimatorParameters.outlier_rejection_subsequent_threshold_degrees_); + declare_parameter("rotational_estimator/outlier_rejection_subsequent_maximum_count", 50); + get_parameter( + "rotational_estimator/outlier_rejection_subsequent_maximum_count", + rotationalEstimatorParameters.outlier_rejection_subsequent_maximum_count_); + + declare_parameter("rotational_estimator/output_minimal_quaternions", false); + get_parameter( + "rotational_estimator/output_minimal_quaternions", + rotationalEstimatorParameters.output_minimal_quaternions_); + + declare_parameter("verbose", true); + get_parameter("verbose", verbose_); // Setting parameters in estimator - vicon_estimator_.setParameters(translationalEstimatorParameters, - rotationalEstimatorParameters); + vicon_estimator_.setParameters(translationalEstimatorParameters, rotationalEstimatorParameters); } void ViconOdometryEstimator::reset() { vicon_estimator_.reset(); } -void ViconOdometryEstimator::publishIntermediateResults(ros::Time timestamp) { +void ViconOdometryEstimator::publishIntermediateResults(rclcpp::Time timestamp) +{ // TODO(millanea): Publishing of the intermediate results was useful when // constructing the estimator however is not likely to still // be useful. Should be removed. - vicon_estimator::TranslationalEstimatorResults - translational_estimator_results; + vicon_estimator::TranslationalEstimatorResults translational_estimator_results; vicon_estimator::RotationalEstimatorResults rotational_estimator_results; - vicon_estimator_.getIntermediateResults(&translational_estimator_results, - &rotational_estimator_results); + vicon_estimator_.getIntermediateResults( + &translational_estimator_results, &rotational_estimator_results); // Creating estimator message - ros_vrpn_client::viconEstimator msg; + ros_vrpn::msg::ViconEstimator msg; // Attaching the vprn timestamp msg.header.stamp = timestamp; // Writing the measurement to the message object - tf2::toMsg(translational_estimator_results.position_measured_, - msg.pos_measured); + tf2::toMsg(translational_estimator_results.position_measured_, msg.pos_measured); // Writing the old estimates to the message object - tf2::toMsg(translational_estimator_results.position_old_, - msg.pos_old); - tf2::toMsg(translational_estimator_results.velocity_old_, - msg.vel_old); + tf2::toMsg(translational_estimator_results.position_old_, msg.pos_old); + tf2::toMsg(translational_estimator_results.velocity_old_, msg.vel_old); // Posteriori results - tf2::toMsg(translational_estimator_results.position_estimate_, - msg.pos_est); - tf2::toMsg(translational_estimator_results.velocity_estimate_, - msg.vel_est); + tf2::toMsg(translational_estimator_results.position_estimate_, msg.pos_est); + tf2::toMsg(translational_estimator_results.velocity_estimate_, msg.vel_est); // Writing the measurement to the message object msg.quat_measured = tf2::toMsg(rotational_estimator_results.orientation_measured_); @@ -141,62 +153,61 @@ void ViconOdometryEstimator::publishIntermediateResults(ros::Time timestamp) { tf2::toMsg(rotational_estimator_results.rate_old_, msg.omega_old); // Posteriori results msg.quat_est = tf2::toMsg(rotational_estimator_results.orientation_estimate_); - tf2::toMsg(rotational_estimator_results.rate_estimate_, - msg.omega_est); + tf2::toMsg(rotational_estimator_results.rate_estimate_, msg.omega_est); // Data to do with the orientation measurement outlier detection - msg.outlier_flag.data = - rotational_estimator_results.measurement_outlier_flag_; - msg.measurement_flip_flag.data = - rotational_estimator_results.measurement_flip_flag_; - msg.q_Z_Z1_magnitude.data = rotational_estimator_results.q_Z_Z1_magnitude_; - msg.q_Z_B_mahalanobis_distance.data = - rotational_estimator_results.q_Z_B_mahalanobis_distance_; - msg.q_covariance_trace.data = - rotational_estimator_results.q_covariance_trace_; + msg.outlier_flag.data = rotational_estimator_results.measurement_outlier_flag_; + msg.measurement_flip_flag.data = rotational_estimator_results.measurement_flip_flag_; + msg.q_z_z1_magnitude.data = rotational_estimator_results.q_Z_Z1_magnitude_; + msg.q_z_b_mahalanobis_distance.data = rotational_estimator_results.q_Z_B_mahalanobis_distance_; + msg.q_covariance_trace.data = rotational_estimator_results.q_covariance_trace_; // Writing the covariance matrix - msg.covariance.layout.dim.push_back(std_msgs::MultiArrayDimension()); + msg.covariance.layout.dim.push_back(std_msgs::msg::MultiArrayDimension()); msg.covariance.layout.dim[0].size = 6; msg.covariance.layout.dim[0].stride = 36; msg.covariance.layout.dim[0].label = "cov_x"; - msg.covariance.layout.dim.push_back(std_msgs::MultiArrayDimension()); + msg.covariance.layout.dim.push_back(std_msgs::msg::MultiArrayDimension()); msg.covariance.layout.dim[1].size = 6; msg.covariance.layout.dim[1].stride = 1; msg.covariance.layout.dim[1].label = "cov_y"; - msg.covariance.data = - std::vector(rotational_estimator_results.covariance_.data(), - rotational_estimator_results.covariance_.data() + - rotational_estimator_results.covariance_.size()); + msg.covariance.data = std::vector( + rotational_estimator_results.covariance_.data(), + rotational_estimator_results.covariance_.data() + + rotational_estimator_results.covariance_.size()); // Publishing estimator message - publisher_.publish(msg); + publisher_->publish(msg); } void ViconOdometryEstimator::updateEstimate( - const Eigen::Vector3d& position_measured_W, - const Eigen::Quaterniond& orientation_measured_B_W, ros::Time timestamp) { + const Eigen::Vector3d & position_measured_W, const Eigen::Quaterniond & orientation_measured_B_W, + rclcpp::Time timestamp) +{ // Converting the ros time stamp to double - double timestamp_double = timestamp.toSec(); + double timestamp_double = timestamp.seconds(); // Updating the estimates - vicon_estimator_.updateEstimate(position_measured_W, orientation_measured_B_W, - timestamp_double); + vicon_estimator_.updateEstimate(position_measured_W, orientation_measured_B_W, timestamp_double); // Getting the estimator statuses EstimatorStatus rotational_estimator_status; EstimatorStatus translational_estimator_status; - vicon_estimator_.getEstimatorStatuses(&translational_estimator_status, - &rotational_estimator_status); + vicon_estimator_.getEstimatorStatuses( + &translational_estimator_status, &rotational_estimator_status); if (verbose_) { if (translational_estimator_status == EstimatorStatus::RESET) { - ROS_WARN("Estimator crashed and restarted: translational estimator"); + RCLCPP_WARN(this->get_logger(), "Estimator crashed and restarted: translational estimator"); } else if (translational_estimator_status == EstimatorStatus::OUTLIER) { - ROS_WARN("Outlier detected: translational estimator"); + RCLCPP_WARN(this->get_logger(), "Outlier detected: translational estimator"); } if (rotational_estimator_status == EstimatorStatus::RESET) { - ROS_WARN("Estimator crashed and restarted: rotational estimator"); + RCLCPP_WARN(this->get_logger(), "Estimator crashed and restarted: rotational estimator"); } else if (rotational_estimator_status == EstimatorStatus::OUTLIER) { - ROS_WARN("Outlier detected: rotational estimator"); + RCLCPP_WARN(this->get_logger(), "Outlier detected: rotational estimator"); } } } -} +} // namespace vicon_estimator + +#include "rclcpp_components/register_node_macro.hpp" + +RCLCPP_COMPONENTS_REGISTER_NODE(vicon_estimator::ViconOdometryEstimator) From 6dbffafcbd22e6869bdddfaadaaae5f72df5e4b2 Mon Sep 17 00:00:00 2001 From: pascalau Date: Sun, 15 Jan 2023 11:20:03 +0100 Subject: [PATCH 2/9] Dropped unnecessary dependencies --- package.xml | 2 -- 1 file changed, 2 deletions(-) diff --git a/package.xml b/package.xml index 5356ec5..a40b5b2 100644 --- a/package.xml +++ b/package.xml @@ -31,8 +31,6 @@ geometry_msgs nav_msgs vrpn_vendor - cmake_modules - message_runtime ament_lint_auto ament_clang_format From 2a0170c370249ed8dd2c9255f8d1112cd5b595d1 Mon Sep 17 00:00:00 2001 From: pascalau Date: Sun, 15 Jan 2023 11:41:50 +0100 Subject: [PATCH 3/9] Formatting --- include/vicon_odometry_estimator.hpp | 4 +++- src/ros_vrpn_client.cpp | 2 +- src/vicon_estimation_node.cpp | 17 +++++++++-------- src/wrapper/vicon_odometry_estimator.cpp | 3 ++- 4 files changed, 15 insertions(+), 11 deletions(-) diff --git a/include/vicon_odometry_estimator.hpp b/include/vicon_odometry_estimator.hpp index feccb6c..995e5f2 100644 --- a/include/vicon_odometry_estimator.hpp +++ b/include/vicon_odometry_estimator.hpp @@ -42,7 +42,9 @@ class ViconOdometryEstimator : public rclcpp::Node { public: // Constructor - ViconOdometryEstimator(const rclcpp::NodeOptions & options, const std::string & node_name="vicon_odometry_estimator"); + ViconOdometryEstimator( + const rclcpp::NodeOptions & options, + const std::string & node_name = "vicon_odometry_estimator"); // Initialize the estimator parameters void initializeParameters(); diff --git a/src/ros_vrpn_client.cpp b/src/ros_vrpn_client.cpp index 6b324e9..b7e3def 100644 --- a/src/ros_vrpn_client.cpp +++ b/src/ros_vrpn_client.cpp @@ -357,7 +357,7 @@ void VRPN_CALLBACK track_target(void * nh_ptr, const vrpn_TRACKERCB tracker) // Timestamping the incomming message const int kMicroSecToNanoSec = 1000; // Changed time source to RCL_ROS_TIME to avoid errors while calculating - // delay. + // delay. rclcpp::Time tracker_timestamp( tracker.msg_time.tv_sec, tracker.msg_time.tv_usec * kMicroSecToNanoSec, RCL_ROS_TIME); rclcpp::Time timestamp; diff --git a/src/vicon_estimation_node.cpp b/src/vicon_estimation_node.cpp index df01cc4..b445dd0 100644 --- a/src/vicon_estimation_node.cpp +++ b/src/vicon_estimation_node.cpp @@ -24,15 +24,14 @@ // finally publishes position, velcocity, orientation and angular // velocity estimates. +#include #include +#include +#include #include #include #include -#include -#include -#include - #include "vicon_odometry_estimator.hpp" using std::placeholders::_1; @@ -45,14 +44,15 @@ class ViconDataListener : public rclcpp::Node ViconDataListener( const rclcpp::NodeOptions & options, const std::shared_ptr vicon_odometry_estimator) - : Node("vicon_data_listener" , options), vicon_odometry_estimator_(vicon_odometry_estimator) + : Node("vicon_data_listener", options), vicon_odometry_estimator_(vicon_odometry_estimator) { // Getting the object name this->declare_parameter("object_name", "auk"); this->get_parameter("object_name", object_name_); // Subscribing to the raw vicon data raw_transform_sub_ = this->create_subscription( - "vrpn_client/raw_transform", 10, std::bind(&ViconDataListener::transformStampedCallback, this, _1)); + "vrpn_client/raw_transform", 10, + std::bind(&ViconDataListener::transformStampedCallback, this, _1)); // Advertising the estimated target state components estimated_transform_pub_ = create_publisher("estimated_transform", 10); @@ -120,7 +120,8 @@ int main(int argc, char ** argv) rclcpp::NodeOptions options; // Creating a Vicon-based estimator to do the estimation - auto vicon_odometry_estimator = std::make_shared(options); + auto vicon_odometry_estimator = + std::make_shared(options); vicon_odometry_estimator->initializeParameters(); vicon_odometry_estimator->reset(); @@ -132,7 +133,7 @@ int main(int argc, char ** argv) rclcpp::executors::SingleThreadedExecutor exec; exec.add_node(vicon_odometry_estimator); exec.add_node(vicon_data_listener); - + exec.spin(); rclcpp::shutdown(); diff --git a/src/wrapper/vicon_odometry_estimator.cpp b/src/wrapper/vicon_odometry_estimator.cpp index 3ee62d6..72e24fa 100644 --- a/src/wrapper/vicon_odometry_estimator.cpp +++ b/src/wrapper/vicon_odometry_estimator.cpp @@ -26,7 +26,8 @@ namespace vicon_estimator { -ViconOdometryEstimator::ViconOdometryEstimator(const rclcpp::NodeOptions & options, const std::string & node_name) +ViconOdometryEstimator::ViconOdometryEstimator( + const rclcpp::NodeOptions & options, const std::string & node_name) : Node(node_name, options), vicon_estimator_(), verbose_(kDefaultVerboseFlag) { // Creating publisher for intermediate estimator values From 041bc8920a2f8d4d214668cbac4eb01d475d2b57 Mon Sep 17 00:00:00 2001 From: pascalau Date: Sun, 15 Jan 2023 11:58:14 +0100 Subject: [PATCH 4/9] Slight formatting --- CMakeLists.txt | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index a899c08..7a84b66 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -74,7 +74,7 @@ ament_export_dependencies( # Compile ros_vrpn_client add_executable(ros_vrpn_client - src/ros_vrpn_client.cpp + src/ros_vrpn_client.cpp ) target_include_directories(ros_vrpn_client @@ -97,7 +97,7 @@ ament_target_dependencies(ros_vrpn_client # Compile vicon_estimation_node add_executable(vicon_estimation_node - src/vicon_estimation_node.cpp + src/vicon_estimation_node.cpp ) target_include_directories(vicon_estimation_node From ec02defbf781bd1c116c815c6d6af9b6eec8c44d Mon Sep 17 00:00:00 2001 From: pascalau Date: Mon, 16 Jan 2023 10:53:16 +0100 Subject: [PATCH 5/9] Added GTests back --- CMakeLists.txt | 11 +++++++---- src/test/CMakeLists.txt | 13 ++++++------- src/test/test_vicon_estimator.cpp | 2 +- 3 files changed, 14 insertions(+), 12 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 7a84b66..646088b 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -118,9 +118,6 @@ ament_target_dependencies(vicon_estimation_node geometry_msgs ) -# TODO(): Add tests back -# add_subdirectory(src/test) - # Install targets install( TARGETS vicon_estimator @@ -139,8 +136,14 @@ install(TARGETS # Testing if(BUILD_TESTING) - # Test linting + find_package(ament_lint_auto REQUIRED) + find_package(ament_cmake_gtest REQUIRED) + + # Unit tests + add_subdirectory(src/test) + + # Test linting ament_lint_auto_find_test_dependencies() endif() diff --git a/src/test/CMakeLists.txt b/src/test/CMakeLists.txt index 218b2ef..a15047a 100644 --- a/src/test/CMakeLists.txt +++ b/src/test/CMakeLists.txt @@ -1,7 +1,6 @@ -if (CATKIN_ENABLE_TESTING) - catkin_add_gtest(test_vicon_estimator test_vicon_estimator.cpp) - cs_add_library(test_helper_library library/test_helper_library.cpp) - include_directories(include) - target_link_libraries(test_vicon_estimator ${catkin_LIBRARIES} vicon_estimator test_helper_library) - SET_TARGET_PROPERTIES(test_vicon_estimator PROPERTIES COMPILE_FLAGS "-std=c++11") -endif() \ No newline at end of file +add_library(test_helper_library library/test_helper_library.cpp) +ament_target_dependencies(test_helper_library Eigen3) +ament_add_gtest(test_vicon_estimator test_vicon_estimator.cpp) +ament_target_dependencies(test_vicon_estimator Eigen3) +target_link_libraries(test_vicon_estimator vicon_estimator test_helper_library) +include_directories(include) diff --git a/src/test/test_vicon_estimator.cpp b/src/test/test_vicon_estimator.cpp index 50db549..80d5a65 100644 --- a/src/test/test_vicon_estimator.cpp +++ b/src/test/test_vicon_estimator.cpp @@ -27,7 +27,7 @@ #include #include "test_helper_library.h" -#include "vicon_estimator.h" +#include "vicon_estimator.hpp" // Translational trajectory defines #define TRANS_TRAJECTORY_PERIOD 10.0 From 2e4dff12a5c892c6830645fcdc1d41fb2350d7e0 Mon Sep 17 00:00:00 2001 From: pascalau Date: Wed, 25 Jan 2023 11:41:09 +0100 Subject: [PATCH 6/9] Python launchfile + minor timing fix --- CMakeLists.txt | 2 +- config/asl_vicon.yaml | 20 ++++++++++++++++ launch/roboa_vicon.launch.py | 46 ++++++++++++++++++++++++++++++++++++ src/ros_vrpn_client.cpp | 4 ++-- 4 files changed, 69 insertions(+), 3 deletions(-) create mode 100644 config/asl_vicon.yaml create mode 100644 launch/roboa_vicon.launch.py diff --git a/CMakeLists.txt b/CMakeLists.txt index 646088b..430e7ff 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -126,7 +126,7 @@ install( RUNTIME DESTINATION bin) install(DIRECTORY - launch + launch config DESTINATION share/${PROJECT_NAME} ) diff --git a/config/asl_vicon.yaml b/config/asl_vicon.yaml new file mode 100644 index 0000000..60595ad --- /dev/null +++ b/config/asl_vicon.yaml @@ -0,0 +1,20 @@ +ros_vrpn_client: + ros__parameters: + vrpn_server_ip: vicon + vrpn_coordinate_system: vicon + timestamping_system: ros + object_name: bluebird + verbose: true + translational_estimator/kp: 0.5 + translational_estimator/kv: 0.5 + rotational_estimator/orientation_estimate_initial_covariance: 100.0 + rotational_estimator/rate_estimate_initial_covariance: 100.0 + rotational_estimator/orientation_process_covariance: 0.000002 + rotational_estimator/rate_process_covariance: 10.0 + rotational_estimator/orientation_measurementCovariance: 0.001 + rotational_estimator/outlier_rejection_method: mahalanobis_distance + rotational_estimator/outlier_rejection_mahalanobis_threshold: 4.0 + rotational_estimator/outlier_rejection_subsequent_threshold_degrees: 30.0 + rotational_estimator/outlier_rejection_subsequent_maximum_count: 50 + rotational_estimator/output_minimal_quaternions: false + capability_group: Vicon diff --git a/launch/roboa_vicon.launch.py b/launch/roboa_vicon.launch.py new file mode 100644 index 0000000..13ed69d --- /dev/null +++ b/launch/roboa_vicon.launch.py @@ -0,0 +1,46 @@ +# Copyright 2023 ASL + +import os + +from ament_index_python.packages import get_package_share_directory + +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument +from launch.substitutions import Command, LaunchConfiguration + +from launch_ros.actions import ComposableNodeContainer, Node +from launch_ros.descriptions import ComposableNode + +import yaml + +object_names = ["roboa_base", "roboa_link_1", "roboa_link_2", "roboa_head"] + + +def get_params(package, file, name): + path = os.path.join(get_package_share_directory(package) + file) + + with open(path, "r") as file: + params = yaml.safe_load(file)[name]["ros__parameters"] + return params + + +def generate_launch_description(): + + params = get_params("ros_vrpn", + "/config/asl_vicon.yaml", + "ros_vrpn_client") + + nodes = [] + for object in object_names: + params["object_name"] = object + nodes.append( + Node( + package="ros_vrpn", + executable="ros_vrpn_client", + namespace=object, + output="screen", + parameters = [params], + ) + ) + + return LaunchDescription(nodes) diff --git a/src/ros_vrpn_client.cpp b/src/ros_vrpn_client.cpp index b7e3def..36e24e1 100644 --- a/src/ros_vrpn_client.cpp +++ b/src/ros_vrpn_client.cpp @@ -302,7 +302,7 @@ void inline getTimeStamp( // timestamp double time_correction_s = time_diff_h_round * kHoursToSec; rclcpp::Time vicon_stamp_corrected( - vicon_stamp.seconds() - time_correction_s, vicon_stamp.nanoseconds()); + vicon_stamp.seconds() - time_correction_s, vicon_stamp.nanoseconds(), RCL_ROS_TIME); // Attaching the corrected timestamp *timestamp = vicon_stamp_corrected; // Outputting the time delay to the ROS console @@ -360,7 +360,7 @@ void VRPN_CALLBACK track_target(void * nh_ptr, const vrpn_TRACKERCB tracker) // delay. rclcpp::Time tracker_timestamp( tracker.msg_time.tv_sec, tracker.msg_time.tv_usec * kMicroSecToNanoSec, RCL_ROS_TIME); - rclcpp::Time timestamp; + rclcpp::Time timestamp = nh->get_clock()->now(); getTimeStamp(nh, tracker_timestamp, ×tamp); // Updating the estimates with the new measurements. From 3928fef8b94cabb9f38e7f1621762d07ac405206 Mon Sep 17 00:00:00 2001 From: pascalau Date: Wed, 25 Jan 2023 14:36:59 +0100 Subject: [PATCH 7/9] cleanup and multi_object launchfile --- ...boa_vicon.launch.py => multi_object.launch.py} | 15 ++++++--------- src/ros_vrpn_client.cpp | 2 +- 2 files changed, 7 insertions(+), 10 deletions(-) rename launch/{roboa_vicon.launch.py => multi_object.launch.py} (61%) diff --git a/launch/roboa_vicon.launch.py b/launch/multi_object.launch.py similarity index 61% rename from launch/roboa_vicon.launch.py rename to launch/multi_object.launch.py index 13ed69d..e91ec2e 100644 --- a/launch/roboa_vicon.launch.py +++ b/launch/multi_object.launch.py @@ -5,15 +5,12 @@ from ament_index_python.packages import get_package_share_directory from launch import LaunchDescription -from launch.actions import DeclareLaunchArgument -from launch.substitutions import Command, LaunchConfiguration -from launch_ros.actions import ComposableNodeContainer, Node -from launch_ros.descriptions import ComposableNode +from launch_ros.actions import Node import yaml -object_names = ["roboa_base", "roboa_link_1", "roboa_link_2", "roboa_head"] +object_names = ["object_1", "object_2", "object_3", "object_4"] def get_params(package, file, name): @@ -26,9 +23,9 @@ def get_params(package, file, name): def generate_launch_description(): - params = get_params("ros_vrpn", - "/config/asl_vicon.yaml", - "ros_vrpn_client") + params = get_params( + "ros_vrpn", "/config/asl_vicon.yaml", "ros_vrpn_client" + ) nodes = [] for object in object_names: @@ -39,7 +36,7 @@ def generate_launch_description(): executable="ros_vrpn_client", namespace=object, output="screen", - parameters = [params], + parameters=[params], ) ) diff --git a/src/ros_vrpn_client.cpp b/src/ros_vrpn_client.cpp index 36e24e1..82057e7 100644 --- a/src/ros_vrpn_client.cpp +++ b/src/ros_vrpn_client.cpp @@ -360,7 +360,7 @@ void VRPN_CALLBACK track_target(void * nh_ptr, const vrpn_TRACKERCB tracker) // delay. rclcpp::Time tracker_timestamp( tracker.msg_time.tv_sec, tracker.msg_time.tv_usec * kMicroSecToNanoSec, RCL_ROS_TIME); - rclcpp::Time timestamp = nh->get_clock()->now(); + rclcpp::Time timestamp; getTimeStamp(nh, tracker_timestamp, ×tamp); // Updating the estimates with the new measurements. From 2c0ae35fc72bc182dcb2e434a94c82f7cdb280c8 Mon Sep 17 00:00:00 2001 From: pascalau Date: Wed, 25 Jan 2023 16:31:38 +0100 Subject: [PATCH 8/9] Fix error with timestamps --- src/ros_vrpn_client.cpp | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/src/ros_vrpn_client.cpp b/src/ros_vrpn_client.cpp index 82057e7..e0b6d68 100644 --- a/src/ros_vrpn_client.cpp +++ b/src/ros_vrpn_client.cpp @@ -302,13 +302,13 @@ void inline getTimeStamp( // timestamp double time_correction_s = time_diff_h_round * kHoursToSec; rclcpp::Time vicon_stamp_corrected( - vicon_stamp.seconds() - time_correction_s, vicon_stamp.nanoseconds(), RCL_ROS_TIME); + vicon_stamp.seconds() - time_correction_s, 0, RCL_ROS_TIME); // Attaching the corrected timestamp *timestamp = vicon_stamp_corrected; // Outputting the time delay to the ROS console if (nh->display_time_delay) { rclcpp::Duration time_diff_corrected = ros_stamp - vicon_stamp_corrected; - static const int kMaxMessagePeriod = 2; + static const int kMaxMessagePeriod = 2000; RCLCPP_WARN_STREAM_THROTTLE( nh->get_logger(), *nh->get_clock(), kMaxMessagePeriod, "Time delay: " << time_diff_corrected.seconds()); @@ -439,6 +439,7 @@ int main(int argc, char * argv[]) tool->publish_estimated_odometry(target_state); fresh_data = false; } + exec.spin_some(); loop_rate.sleep(); } From bb58310fb5b06a84eb15b14b3ce085886e7010d9 Mon Sep 17 00:00:00 2001 From: pascalau Date: Wed, 25 Jan 2023 17:25:37 +0100 Subject: [PATCH 9/9] Fix issue with numerical precision in stamp --- src/ros_vrpn_client.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/ros_vrpn_client.cpp b/src/ros_vrpn_client.cpp index e0b6d68..757c1d7 100644 --- a/src/ros_vrpn_client.cpp +++ b/src/ros_vrpn_client.cpp @@ -302,7 +302,7 @@ void inline getTimeStamp( // timestamp double time_correction_s = time_diff_h_round * kHoursToSec; rclcpp::Time vicon_stamp_corrected( - vicon_stamp.seconds() - time_correction_s, 0, RCL_ROS_TIME); + vicon_stamp.nanoseconds() - time_correction_s * 1e9, RCL_ROS_TIME); // Attaching the corrected timestamp *timestamp = vicon_stamp_corrected; // Outputting the time delay to the ROS console