From b577932ee5182376565512925aba127c02185053 Mon Sep 17 00:00:00 2001 From: qishuny Date: Fri, 3 Jun 2022 18:47:13 -0400 Subject: [PATCH] Fixed params loading issue. Rewrite comments --- robot_driver/CMakeLists.txt | 2 +- .../estimators/comp_filter_estimator.h | 8 +++--- ...ekf_filter_estimator.h => ekf_estimator.h} | 12 ++++---- .../robot_driver/estimators/state_estimator.h | 28 ++++++++----------- .../include/robot_driver/robot_driver.h | 2 +- .../src/estimators/comp_filter_estimator.cpp | 18 ++++++------ ...filter_estimator.cpp => ekf_estimator.cpp} | 2 +- 7 files changed, 34 insertions(+), 38 deletions(-) rename robot_driver/include/robot_driver/estimators/{ekf_filter_estimator.h => ekf_estimator.h} (73%) rename robot_driver/src/estimators/{ekf_filter_estimator.cpp => ekf_estimator.cpp} (83%) diff --git a/robot_driver/CMakeLists.txt b/robot_driver/CMakeLists.txt index 0ed5a6612..533813c16 100644 --- a/robot_driver/CMakeLists.txt +++ b/robot_driver/CMakeLists.txt @@ -67,7 +67,7 @@ add_library(robot_driver src/controllers/joint_controller.cpp src/estimators/state_estimator.cpp src/estimators/comp_filter_estimator.cpp - src/estimators/ekf_filter_estimator.cpp + src/estimators/ekf_estimator.cpp src/hardware_interfaces/hardware_interface.cpp src/hardware_interfaces/spirit_interface.cpp ) diff --git a/robot_driver/include/robot_driver/estimators/comp_filter_estimator.h b/robot_driver/include/robot_driver/estimators/comp_filter_estimator.h index 2f7695f42..6c89cb973 100644 --- a/robot_driver/include/robot_driver/estimators/comp_filter_estimator.h +++ b/robot_driver/include/robot_driver/estimators/comp_filter_estimator.h @@ -13,19 +13,19 @@ class CompFilterEstimator : public StateEstimator { CompFilterEstimator(); /** - * @brief initialize Complementary Filter - * @param[in] nh ros::NodeHandle to load parameters from yaml file + * @brief Initialize Complementary Filter + * @param[in] nh ROS Node Handler used to load parameters from yaml file */ void init(ros::NodeHandle& nh); /** - * @brief helper function to filter mocap data + * @brief Helper function to filter mocap data */ void mocapCallBackHelper(const geometry_msgs::PoseStamped::ConstPtr& msg, const Eigen::Vector3d& pos); /** - * @brief perform CF update once + * @brief Perform CF update once * @param[out] last_robot_state_msg */ bool updateOnce(quad_msgs::RobotState& last_robot_state_msg); diff --git a/robot_driver/include/robot_driver/estimators/ekf_filter_estimator.h b/robot_driver/include/robot_driver/estimators/ekf_estimator.h similarity index 73% rename from robot_driver/include/robot_driver/estimators/ekf_filter_estimator.h rename to robot_driver/include/robot_driver/estimators/ekf_estimator.h index 7bfde19e0..5815fec1b 100644 --- a/robot_driver/include/robot_driver/estimators/ekf_filter_estimator.h +++ b/robot_driver/include/robot_driver/estimators/ekf_estimator.h @@ -1,5 +1,5 @@ -#ifndef EKF_FILTER_H -#define EKF_FILTER_H +#ifndef EKF_H +#define EKF_H #include @@ -13,13 +13,13 @@ class EKFEstimator : public StateEstimator { EKFEstimator(); /** - * @brief initialize EKF - * @param[in] nh ros::NodeHandle to load parameters from yaml file + * @brief Initialize EKF + * @param[in] nh Node Handler to load parameters from yaml file */ void init(ros::NodeHandle& nh); /** - * @brief perform EKF update once + * @brief Perform EKF update once * @param[out] last_robot_state_msg_ */ bool updateOnce(quad_msgs::RobotState& last_robot_state_msg_); @@ -29,4 +29,4 @@ class EKFEstimator : public StateEstimator { ros::NodeHandle nh_; }; -#endif // EKF_FILTER_H +#endif // EKF_H diff --git a/robot_driver/include/robot_driver/estimators/state_estimator.h b/robot_driver/include/robot_driver/estimators/state_estimator.h index f33d7ce9b..478d89d1a 100644 --- a/robot_driver/include/robot_driver/estimators/state_estimator.h +++ b/robot_driver/include/robot_driver/estimators/state_estimator.h @@ -10,9 +10,6 @@ #include #include -#include "robot_driver/hardware_interfaces/hardware_interface.h" -#include "robot_driver/hardware_interfaces/spirit_interface.h" - //! Implements an abstract class for state estimator. //! This class provides an interface for different types of estimators /*! @@ -27,16 +24,16 @@ class StateEstimator { StateEstimator(); /** - * @brief virtual function for initialize filters, should be defined in + * @brief Virtual function for initialize filters, should be defined in * derived class - * @param[in] nh_ sensor_msgs::Imu::ConstPtr imu sensor message + * @param[in] nh_ ROS Node Ha */ virtual void init(ros::NodeHandle& nh) = 0; /** - * @brief virtual update function for update robot state, should be defined in + * @brief Virtual update function for update robot state, should be defined in * derived class - * @param[out] last_robot_state_msg quad_msgs::RobotState robot state + * @param[out] last_robot_state_msg robot state */ virtual bool updateOnce(quad_msgs::RobotState& last_robot_state_msg) = 0; @@ -52,25 +49,24 @@ class StateEstimator { Eigen::Quaterniond& qk); /** - * @brief read joint encoder data - * @param[in] last_joint_state_msg sensor_msgs::JointState::ConstPtr joint - * state sensor message - * @param[out] jk Eigen::VectorXd jointstate (12 * 1) + * @brief Read joint encoder data + * @param[in] last_joint_state_msg Joint state sensor message + * @param[out] jk Jointstate in vector (12 * 1) */ void readJointEncoder( const sensor_msgs::JointState::ConstPtr& last_joint_state_msg, Eigen::VectorXd& jk); /** - * @brief load Mocap data to protected variable - * @param[in] last_mocap_msg geometry_msgs::PoseStamped::ConstPtr + * @brief Load Mocap data to protected variable + * @param[in] last_mocap_msg Mocap message */ void loadMocapMsg(geometry_msgs::PoseStamped::ConstPtr last_mocap_msg); /** - * @brief load imu and joint encoder data to protected variables - * @param[in] last_imu_msg sensor_msgs::Imu - * @param[in] last_joint_state_msg sensor_msgs::JointState + * @brief Load imu and joint encoder data to protected variables + * @param[in] last_imu_msg imu msgs + * @param[in] last_joint_state_msg joint state msgs */ void loadSensorMsg(sensor_msgs::Imu last_imu_msg, sensor_msgs::JointState last_joint_state_msg); diff --git a/robot_driver/include/robot_driver/robot_driver.h b/robot_driver/include/robot_driver/robot_driver.h index 67721aacc..0e82f2f23 100644 --- a/robot_driver/include/robot_driver/robot_driver.h +++ b/robot_driver/include/robot_driver/robot_driver.h @@ -24,7 +24,7 @@ #include "robot_driver/controllers/joint_controller.h" #include "robot_driver/controllers/leg_controller.h" #include "robot_driver/estimators/comp_filter_estimator.h" -#include "robot_driver/estimators/ekf_filter_estimator.h" +#include "robot_driver/estimators/ekf_estimator.h" #include "robot_driver/estimators/state_estimator.h" #include "robot_driver/hardware_interfaces/hardware_interface.h" #include "robot_driver/hardware_interfaces/spirit_interface.h" diff --git a/robot_driver/src/estimators/comp_filter_estimator.cpp b/robot_driver/src/estimators/comp_filter_estimator.cpp index deafc2ace..cda8f96ce 100644 --- a/robot_driver/src/estimators/comp_filter_estimator.cpp +++ b/robot_driver/src/estimators/comp_filter_estimator.cpp @@ -5,15 +5,15 @@ CompFilterEstimator::CompFilterEstimator() {} void CompFilterEstimator::init(ros::NodeHandle& nh) { nh_ = nh; // load Comp_filter params - quad_utils::loadROSParam(nh_, "robot_driver/high_pass_a", high_pass_a_); - quad_utils::loadROSParam(nh_, "robot_driver/high_pass_b", high_pass_b_); - quad_utils::loadROSParam(nh_, "robot_driver/high_pass_c", high_pass_c_); - quad_utils::loadROSParam(nh_, "robot_driver/high_pass_d", high_pass_d_); - - quad_utils::loadROSParam(nh_, "robot_driver/low_pass_a", low_pass_a_); - quad_utils::loadROSParam(nh_, "robot_driver/low_pass_b", low_pass_b_); - quad_utils::loadROSParam(nh_, "robot_driver/low_pass_c", low_pass_c_); - quad_utils::loadROSParam(nh_, "robot_driver/low_pass_d", low_pass_d_); + quad_utils::loadROSParam(nh_, "/robot_driver/high_pass_a", high_pass_a_); + quad_utils::loadROSParam(nh_, "/robot_driver/high_pass_b", high_pass_b_); + quad_utils::loadROSParam(nh_, "/robot_driver/high_pass_c", high_pass_c_); + quad_utils::loadROSParam(nh_, "/robot_driver/high_pass_d", high_pass_d_); + + quad_utils::loadROSParam(nh_, "/robot_driver/low_pass_a", low_pass_a_); + quad_utils::loadROSParam(nh_, "/robot_driver/low_pass_b", low_pass_b_); + quad_utils::loadROSParam(nh_, "/robot_driver/low_pass_c", low_pass_c_); + quad_utils::loadROSParam(nh_, "/robot_driver/low_pass_d", low_pass_d_); high_pass_filter.A = Eigen::Map >(high_pass_a_.data()).transpose(); high_pass_filter.B = diff --git a/robot_driver/src/estimators/ekf_filter_estimator.cpp b/robot_driver/src/estimators/ekf_estimator.cpp similarity index 83% rename from robot_driver/src/estimators/ekf_filter_estimator.cpp rename to robot_driver/src/estimators/ekf_estimator.cpp index d3cc4744b..7ace6584b 100644 --- a/robot_driver/src/estimators/ekf_filter_estimator.cpp +++ b/robot_driver/src/estimators/ekf_estimator.cpp @@ -1,4 +1,4 @@ -#include "robot_driver/estimators/ekf_filter_estimator.h" +#include "robot_driver/estimators/ekf_estimator.h" EKFEstimator::EKFEstimator() {}