measure_parameter.hpp

Go to the documentation of this file.

 
#ifndef _MEASURE_PARAMETER_HPP
#define _MEASURE_PARAMETER_HPP
 
#include <geometry_msgs/msg/wrench_stamped.hpp>
#include <localization_msgs/msg/odometry.hpp>
#include <p_pid_controller/p_pid_controller.hpp>
#include <rclcpp/rclcpp.hpp>
 
namespace controller
{
 
const uint buf_size = 20;
 
class MeasureParam : public rclcpp::Node
{
private:
  rclcpp::Publisher<geometry_msgs::msg::WrenchStamped>::SharedPtr pub_;
  rclcpp::Subscription<localization_msgs::msg::Odometry>::SharedPtr sub_odom_;
 
  rclcpp::TimerBase::SharedPtr timer_;
 
  std::shared_ptr<localization_msgs::msg::Odometry> odom_;
 
  std::shared_ptr<controller::P_PIDController> p_pid_ctrl_;
 
  const int timeout = 30;  // [s]
 
  std::string yaml_path;
 
  double target_z_depth = 0;
  double target_roll = 0;
  double force_z = 0;
  double torque_roll = 0;
  double param_z = 0;
  double param_roll = 0;
 
  std::array<double, buf_size> buf_sens = {0};
  std::array<double, buf_size> buf_out = {0};
 
  bool z_measuring = false;
  bool z_measured = false;
  bool roll_measuring = false;
  bool roll_measured = false;
  size_t count = buf_size;
  rclcpp::Time start_time;
 
  void _save_yaml();
 
  void callback_odom(localization_msgs::msg::Odometry::UniquePtr msg);
 
  void pid_update();
 
  bool _measure_z();
 
  bool _measure_roll();
 
  void measure();
 
public:
  explicit MeasureParam(const rclcpp::NodeOptions & options);
};
 
}  // namespace controller
 
#endif