change function input, fix lint

robomechanics · May 25, 2022 · 82c665d · 82c665d
1 parent 05a077a
commit 82c665d
Show file tree

Hide file tree

Showing 3 changed files with 13 additions and 18 deletions.
diff --git a/robot_driver/include/robot_driver/estimators/comp_filter_estimator.h b/robot_driver/include/robot_driver/estimators/comp_filter_estimator.h
@@ -44,8 +44,8 @@ class CompFilterEstimator : public StateEstimator {
   void mocapCallBackHelper(const geometry_msgs::PoseStamped::ConstPtr& msg,
                            const Eigen::Vector3d& pos,
                            geometry_msgs::PoseStamped::ConstPtr last_mocap_msg_,
-                           const double mocap_rate_,
-                           const double mocap_dropout_threshold_);
+                           const double& mocap_rate_,
+                           const double& mocap_dropout_threshold_);
 
   /**
    * @brief update CF state once
@@ -54,19 +54,17 @@ class CompFilterEstimator : public StateEstimator {
 
   /**
    * @brief update CF state once
-   * @param[in] hardware_interface_
+   * @param[in] fully_populated
    * @param[in] last_imu_msg_
    * @param[in] last_joint_state_msg_
    * @param[in] last_mocap_msg_
    * @param[in] user_rx_data_
    * @param[out] last_robot_state_msg_
    */
 
-  bool updateState(std::shared_ptr<HardwareInterface> hardware_interface_,
-                   sensor_msgs::Imu& last_imu_msg_,
+  bool updateState(const bool& fully_populated, sensor_msgs::Imu& last_imu_msg_,
                    sensor_msgs::JointState& last_joint_state_msg_,
                    geometry_msgs::PoseStamped::ConstPtr last_mocap_msg_,
-                   Eigen::VectorXd& user_rx_data_,
                    quad_msgs::RobotState& last_robot_state_msg_);
 
   bool initiated;

diff --git a/robot_driver/src/estimators/comp_filter_estimator.cpp b/robot_driver/src/estimators/comp_filter_estimator.cpp
@@ -54,17 +54,11 @@ bool CompFilterEstimator::updateState() {
 }
 
 bool CompFilterEstimator::updateState(
-    std::shared_ptr<HardwareInterface> hardware_interface_,
-    sensor_msgs::Imu& last_imu_msg_,
+    const bool& fully_populated, sensor_msgs::Imu& last_imu_msg_,
     sensor_msgs::JointState& last_joint_state_msg_,
     geometry_msgs::PoseStamped::ConstPtr last_mocap_msg_,
-    Eigen::VectorXd& user_rx_data_,
     quad_msgs::RobotState& last_robot_state_msg_) {
   std::cout << "CF Estimator Updated Once" << std::endl;
-  // Get the newest data from the robot (BLOCKING)
-  bool fully_populated = hardware_interface_->recv(
-      last_joint_state_msg_, last_imu_msg_, user_rx_data_);
-
   ros::Time state_timestamp = ros::Time::now();
 
   // Check if robot data was recieved
@@ -147,7 +141,7 @@ bool CompFilterEstimator::updateState(
 void CompFilterEstimator::mocapCallBackHelper(
     const geometry_msgs::PoseStamped::ConstPtr& msg, const Eigen::Vector3d& pos,
     geometry_msgs::PoseStamped::ConstPtr last_mocap_msg_,
-    const double mocap_rate_, const double mocap_dropout_threshold_) {
+    const double& mocap_rate_, const double& mocap_dropout_threshold_) {
   if (low_pass_filter.init) {
     // Record time diff between messages
     ros::Time t_now = ros::Time::now();
@@ -191,4 +185,4 @@ void CompFilterEstimator::mocapCallBackHelper(
 
     low_pass_filter.init = true;
   }
-};
+}
diff --git a/robot_driver/src/robot_driver.cpp b/robot_driver/src/robot_driver.cpp
@@ -160,7 +160,7 @@ void RobotDriver::initStateEstimator() {
   if (estimator_id_ == "comp_filter") {
     comp_filter_estimator_ = std::make_shared<CompFilterEstimator>();
   } else if (estimator_id_ == "ekf_filter") {
-    state_estimator_ = std::make_shared<EKFFilterEstimator>();
+    ekf_estimatror_ = std::make_shared<EKFFilterEstimator>();
   } else {
     ROS_ERROR_STREAM("Invalid estimator id " << estimator_id_
                                              << ", returning nullptr");
@@ -299,9 +299,12 @@ void RobotDriver::checkMessagesForSafety() {
 bool RobotDriver::updateState() {
   if (is_hardware_) {
     if (estimator_id_ == "comp_filter") {
+      // Get the newest data from the robot (BLOCKING)
+      bool fully_populated = hardware_interface_->recv(
+          last_joint_state_msg_, last_imu_msg_, user_rx_data_);
       return comp_filter_estimator_->updateState(
-          hardware_interface_, last_imu_msg_, last_joint_state_msg_,
-          last_mocap_msg_, user_rx_data_, last_robot_state_msg_);
+          fully_populated, last_imu_msg_, last_joint_state_msg_,
+          last_mocap_msg_, last_robot_state_msg_);
     } else if (estimator_id_ == "ekf_filter") {
       return ekf_estimatror_->updateState();
     } else {