[230224] 모터 Wheel tick 데이터 추출 및 publish 하기

개발 진행중 모터의 wheel tick(WH) 데이터가 필요해져 모터 드라이버쪽 코드를 수정해 해결하였다.

 

먼저 사용중인 로봇은 STELLA N1 Rasp4 모델이고 Ros2 foxy를 사용하고있다.

 

모터 드라이버 라이브러리의 기본 코드는 아래 github에서 확인할 수 있다.

https://github.com/ntrexlab/STELLA_N1_PI_X4_ROS2_v2.0.git

GitHub - ntrexlab/STELLA_N1_PI_X4_ROS2_v2.0

 

이제 Wheel tick을 추출해보자 

 

기본적으로 Wheel tick = 엔코더펄스/분해능 으로 구한다.

따라서 해당 소스에서는 모터의 엔코더 펄스와 ppr가 

MyMotorCommandReadValue.position[channel]; // pulse
MyMotorConfiguration.encoder_ppr[channel]; // ppr

로 주어지므로

 

wheel rotation count 는 

double get_wheel_rotation_count(int channel) {
  int count = MyMotorCommandReadValue.position[channel];
  int ppr = MyMotorConfiguration.encoder_ppr[channel];
  return static_cast<double>(count) / static_cast<double>(ppr);
}

로 구할수 있다.

 

이를 main.cpp에 추가해주고 main.hpp 을

#include <std_msgs/msg/int32_multi_array.hpp>

using namespace std::chrono_literals;
using std::placeholders::_1;

class stellaN1_node : public rclcpp::Node
{
public:
    stellaN1_node();
    ~stellaN1_node();

private:

    // ROS timer
    rclcpp::TimerBase::SharedPtr Serial_timer;
    
    // ROS topic publishers
    rclcpp::Publisher<nav_msgs::msg::Odometry>::SharedPtr odom_pub_;
    rclcpp::Publisher<std_msgs::msg::Int32MultiArray>::SharedPtr wheel_tick_pub_;


    // ROS topic subscribers
    rclcpp::Subscription<geometry_msgs::msg::Twist>::SharedPtr cmd_vel_sub_;
    rclcpp::Subscription<std_msgs::msg::Float64>::SharedPtr ahrs_yaw_sub_;
    
    rclcpp::Time time_now;
    
    std::unique_ptr<tf2_ros::TransformBroadcaster> odom_broadcaster;
    
    double goal_linear_velocity_;
    double goal_angular_velocity_;
    
    int left_encoder_prev=0,right_encoder_prev=0;
    
    double ahrs_yaw, delta_th=0.0,delta_s=0.0,delta_x=0.0,delta_y=0.0,x=0.0,y=0.0,th=0.0,delta_left = 0,delta_right = 0;

    void ahrs_yaw_data_callback(const std_msgs::msg::Float64::SharedPtr msg);
    void command_velocity_callback(const geometry_msgs::msg::Twist::SharedPtr cmd_vel_msg);
    void serial_callback();
    bool update_odometry();
};

와 같이 추가해준다.

 

마지막으로 Node 선언 부분에서 

stellaN1_node::stellaN1_node() : Node("stella_md_node")
{
  auto qos = rclcpp::QoS(rclcpp::KeepLast(10));

  ahrs_yaw_sub_ = this->create_subscription<std_msgs::msg::Float64>("imu/yaw", 1, std::bind(&stellaN1_node::ahrs_yaw_data_callback, this, std::placeholders::_1));
  cmd_vel_sub_ = this->create_subscription<geometry_msgs::msg::Twist>("cmd_vel", 10, std::bind(&stellaN1_node::command_velocity_callback, this, std::placeholders::_1));
  odom_pub_ = this->create_publisher<nav_msgs::msg::Odometry>("odom", qos);
  odom_broadcaster = std::make_unique<tf2_ros::TransformBroadcaster>(*this);

  // wheel_tick pub
  wheel_tick_pub_ = this->create_publisher<std_msgs::msg::Int32MultiArray>("wheel_tick", qos);

  Serial_timer = this->create_wall_timer(1ms, std::bind(&stellaN1_node::serial_callback, this));
}

와 같이 publisher를 추가해주고

 

callback 부분에

void stellaN1_node::serial_callback()
{
  if(RUN)
  {
    Motor_MonitoringCommand(channel_1, _position);
    Motor_MonitoringCommand(channel_2, _position);
    
    
    // wheel tick 데이터 pub
    std_msgs::msg::Int32MultiArray wheel_ticks;
    wheel_ticks.data.clear();
    wheel_ticks.data.push_back(get_wheel_rotation_count(channel_1));
    wheel_ticks.data.push_back(get_wheel_rotation_count(channel_2));
    wheel_tick_pub_->publish(wheel_ticks);

    update_odometry();
  }
}

와 같이 추가해준다.

 

수정된 파일을 저장 후 터미널에서 아래 명령어를 통해 빌드한다.

pi@raspberrypi:~/colcon_ws $ colcon build --symlink-install



This may be promoted to an error in a future release of colcon-core.
Starting >>> stella_description
Starting >>> stella_ahrs                         
Starting >>> stella_camera                                                        
Starting >>> stella_md                                                                                   
Finished <<< stella_description [5.08s]                                                                            
Starting >>> stella_teleop_bluetooth
Finished <<< stella_ahrs [7.92s]                                                                                    
Starting >>> ydlidar
Finished <<< stella_md [9.24s]                                                          
Starting >>> stella_bringup
Finished <<< stella_bringup [2.80s]                                                            
Finished <<< ydlidar [4.95s]                                                                                        
Finished <<< stella_camera [12.9s]                                                         
Finished <<< stella_teleop_bluetooth [11.2s]            

Summary: 7 packages finished [17.1s]

빌드를 성공하였다.

 

 

Stella node 실행 후 아래 명령어 입력시 wheel tick 데이터가 좌/우 모터로 나뉘어 출력되는것을 확인 할 수 있다.

ros2 topic echo wheel_tick

 

후진 후 전진 중 wheel tick data