Fixed params loading issue. Rewrite comments

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
 )

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);

robot_driver/include/robot_driver/estimators/ekf_filter_estimator.h → 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 <robot_driver/estimators/state_estimator.h>
 
@@ -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

robot_driver/include/robot_driver/estimators/state_estimator.h

@@ -10,9 +10,6 @@
 #include <sensor_msgs/JointState.h>
 #include <std_msgs/String.h>
 
-#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);

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"

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<Eigen::Matrix<double, 2, 2> >(high_pass_a_.data()).transpose();
   high_pass_filter.B =

robot_driver/src/estimators/ekf_filter_estimator.cpp → 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() {}