Program Listing for File direction_controller_component.hpp

↰ Return to documentation for file (/tmp/ws/src/raspimouse_ros2_examples/include/raspimouse_ros2_examples/direction_controller_component.hpp)

// Copyright 2020 RT Corporation
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
//     http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.

#ifndef RASPIMOUSE_ROS2_EXAMPLES__DIRECTION_CONTROLLER_COMPONENT_HPP_
#define RASPIMOUSE_ROS2_EXAMPLES__DIRECTION_CONTROLLER_COMPONENT_HPP_

#include <memory>
#include <string>
#include <vector>

#include "raspimouse_ros2_examples/visibility_control.h"
#include "raspimouse_msgs/msg/switches.hpp"
#include "rclcpp/rclcpp.hpp"
#include "sensor_msgs/msg/imu.hpp"
#include "std_msgs/msg/int16.hpp"
#include "std_msgs/msg/float64.hpp"
#include "std_msgs/msg/string.hpp"
#include "std_srvs/srv/set_bool.hpp"
#include "geometry_msgs/msg/twist.hpp"
#include "geometry_msgs/msg/vector3.hpp"

namespace direction_controller
{

class PIDController
{
public:
  PIDController()
  : p_gain_(0.0), i_gain_(0.0), d_gain_(0.0),
    error1_(0.0), error2_(0.0), output_(0.0)
  {
  }

  double update(const double current, const double target)
  {
    double error = target - current;

    double delta_output = p_gain_ * (error - error1_);
    delta_output += i_gain_ * (error);
    delta_output += d_gain_ * (error - 2.0 * error1_ + error2_);

    output_ += delta_output;

    error2_ = error1_;
    error1_ = error;

    return output_;
  }

  void set_gain(const double p_gain, const double i_gain, const double d_gain)
  {
    p_gain_ = p_gain;
    i_gain_ = i_gain;
    d_gain_ = d_gain;
  }

  void reset_output_and_errors()
  {
    error1_ = 0.0;
    error2_ = 0.0;
    output_ = 0.0;
  }

private:
  double p_gain_;
  double i_gain_;
  double d_gain_;
  double error1_;
  double error2_;
  double output_;
};

class Controller : public rclcpp::Node
{
public:
  RASPIMOUSE_ROS2_EXAMPLES_PUBLIC
  explicit Controller(const rclcpp::NodeOptions & options);

protected:
  void on_cmd_vel_timer();

private:
  raspimouse_msgs::msg::Switches switches_;
  sensor_msgs::msg::Imu imu_data_raw_;

  rclcpp::Publisher<std_msgs::msg::Int16>::SharedPtr buzzer_pub_;
  rclcpp::Publisher<geometry_msgs::msg::Twist>::SharedPtr cmd_vel_pub_;
  rclcpp::Publisher<std_msgs::msg::Float64>::SharedPtr heading_angle_pub_;
  rclcpp::Subscription<raspimouse_msgs::msg::Switches>::SharedPtr switches_sub_;
  rclcpp::Subscription<sensor_msgs::msg::Imu>::SharedPtr imu_data_raw_sub_;
  rclcpp::Client<std_srvs::srv::SetBool>::SharedPtr motor_power_client_;

  rclcpp::TimerBase::SharedPtr cmd_vel_timer_;

  int pressed_switch_number_;
  int control_mode_;
  PIDController omega_pid_controller_;
  std::vector<double> omega_samples_;
  double omega_bias_;
  double heading_angle_;
  double prev_heading_calculation_time_;
  double target_angle_;
  bool increase_target_angle_;
  geometry_msgs::msg::Vector3 filtered_acc_;
  geometry_msgs::msg::Vector3 prev_acc_;

  void callback_switches(const raspimouse_msgs::msg::Switches::SharedPtr msg);
  void callback_imu_data_raw(const sensor_msgs::msg::Imu::SharedPtr msg);

  bool set_motor_power(const bool motor_on);
  void filter_acceleration(const geometry_msgs::msg::Vector3 acc);
  bool omega_calibration(const double omega);
  void calculate_heading_angle(const double omega, const double current_time);
  void angle_control(const double target_angle);
  void rotation(void);
  void beep_buzzer(const int freq, const std::chrono::nanoseconds & beep_time);
  void beep_start(void);
  void beep_success(void);
  void beep_failure(void);
};

}  // namespace direction_controller

#endif  // RASPIMOUSE_ROS2_EXAMPLES__DIRECTION_CONTROLLER_COMPONENT_HPP_