[KUSMO] 11/19 (Motor Encoder Data Publisher)

모터의 핀을 아래와 같이 연결

1) 왼쪽 모터

• Hall sensor GND -> Arduino GND
• Hall sensor Vcc -> Arduino`s 5V pin
  • Encoder에 전력을 공급하는 핀
• Hall sensor A -> pin 2
  • 2번 pin은 Encoder A로부터 digital rising signal이 들어올 때 마다 그 시간을 저장 
• Hall sensor B -> pin 4
  
  • 4번 pin으로 들어오는 신호는 모터가 앞으로 가는지 뒤로 가는지를 판단

2) 오른쪽 모터

• Hall sensor GND/Vcc는 위와 동일
• Hall sensor A -> pin 3
• Hall sensor B -> pin 11

 

• Arduino IDE 실행 후 아래 코드 입력, upload

// tick_counter
// Encoder output to Arduino Interrupt pin. Tracks the tick count.
#define ENC_IN_LEFT_A 2
#define ENC_IN_RIGHT_A 3
 
// Other encoder output to Arduino to keep track of wheel direction
// Tracks the direction of rotation.
#define ENC_IN_LEFT_B 4
#define ENC_IN_RIGHT_B 11
 
// True = Forward; False = Reverse
boolean Direction_left = true;
boolean Direction_right = true;
 
// Minumum and maximum values for 16-bit integers
const int encoder_minimum = -32768;
const int encoder_maximum = 32767;
 
// Keep track of the number of wheel ticks
volatile int left_wheel_tick_count = 0;
volatile int right_wheel_tick_count = 0;
 
// One-second interval for measurements
int interval = 1000;
long previousMillis = 0;
long currentMillis = 0;
 
void setup() {
 
  // Open the serial port at 9600 bps
  Serial.begin(9600); 
 
  // Set pin states of the encoder
  pinMode(ENC_IN_LEFT_A , INPUT_PULLUP);
  pinMode(ENC_IN_LEFT_B , INPUT);
  pinMode(ENC_IN_RIGHT_A , INPUT_PULLUP);
  pinMode(ENC_IN_RIGHT_B , INPUT);
 
  // Every time the pin goes high, this is a tick
  attachInterrupt(digitalPinToInterrupt(ENC_IN_LEFT_A), left_wheel_tick, RISING);
  attachInterrupt(digitalPinToInterrupt(ENC_IN_RIGHT_A), right_wheel_tick, RISING);
}
 
void loop() {
 
  // Record the time
  currentMillis = millis();
 
  // If one second has passed, print the number of ticks
  if (currentMillis - previousMillis > interval) {
     
    previousMillis = currentMillis;
 
    Serial.println("Number of Ticks: ");
    Serial.println(right_wheel_tick_count);
    Serial.println(left_wheel_tick_count);
    Serial.println();
  }
}
 
// Increment the number of ticks
void right_wheel_tick() {
   
  // Read the value for the encoder for the right wheel
  int val = digitalRead(ENC_IN_RIGHT_B);
 
  if(val == LOW) {
    Direction_right = false; // Reverse
  }
  else {
    Direction_right = true; // Forward
  }
   
  if (Direction_right) {
     
    if (right_wheel_tick_count == encoder_maximum) {
      right_wheel_tick_count = encoder_minimum;
    }
    else {
      right_wheel_tick_count++;  
    }    
  }
  else {
    if (right_wheel_tick_count == encoder_minimum) {
      right_wheel_tick_count = encoder_maximum;
    }
    else {
      right_wheel_tick_count--;  
    }   
  }
}
 
// Increment the number of ticks
void left_wheel_tick() {
   
  // Read the value for the encoder for the left wheel
  int val = digitalRead(ENC_IN_LEFT_B);
 
  if(val == LOW) {
    Direction_left = true; // Reverse
  }
  else {
    Direction_left = false; // Forward
  }
   
  if (Direction_left) {
    if (left_wheel_tick_count == encoder_maximum) {
      left_wheel_tick_count = encoder_minimum;
    }
    else {
      left_wheel_tick_count++;  
    }  
  }
  else {
    if (left_wheel_tick_count == encoder_minimum) {
      left_wheel_tick_count = encoder_maximum;
    }
    else {
      left_wheel_tick_count--;  
    }   
  }
}

• Tick count를 세는 코드
• Serial Monitor에서 누적된 tick count를 볼 수 있음
• Tick count의 범위는  -32768 ~ 32767 (16bit integer type)

 

• 새로운 sketch를 열고 아래의 코드 입력
• 위 내용을 ROS node로 전환하는 코드

//tick_publisher

#include <ros.h>
#include <std_msgs/Int16.h>
 
// Handles startup and shutdown of ROS
ros::NodeHandle nh;
 
// Encoder output to Arduino Interrupt pin. Tracks the tick count.
#define ENC_IN_LEFT_A 2
#define ENC_IN_RIGHT_A 3
 
// Other encoder output to Arduino to keep track of wheel direction
// Tracks the direction of rotation.
#define ENC_IN_LEFT_B 4
#define ENC_IN_RIGHT_B 11
 
// True = Forward; False = Reverse
boolean Direction_left = true;
boolean Direction_right = true;
 
// Minumum and maximum values for 16-bit integers
const int encoder_minimum = -32768;
const int encoder_maximum = 32767;
 
// Keep track of the number of wheel ticks
std_msgs::Int16 right_wheel_tick_count;
ros::Publisher rightPub("right_ticks", &right_wheel_tick_count);
 
std_msgs::Int16 left_wheel_tick_count;
ros::Publisher leftPub("left_ticks", &left_wheel_tick_count);
 
// 100ms interval for measurements
const int interval = 100;
long previousMillis = 0;
long currentMillis = 0;
 
// Increment the number of ticks
void right_wheel_tick() {
   
  // Read the value for the encoder for the right wheel
  int val = digitalRead(ENC_IN_RIGHT_B);
 
  if(val == LOW) {
    Direction_right = false; // Reverse
  }
  else {
    Direction_right = true; // Forward
  }
   
  if (Direction_right) {
     
    if (right_wheel_tick_count.data == encoder_maximum) {
      right_wheel_tick_count.data = encoder_minimum;
    }
    else {
      right_wheel_tick_count.data++;  
    }    
  }
  else {
    if (right_wheel_tick_count.data == encoder_minimum) {
      right_wheel_tick_count.data = encoder_maximum;
    }
    else {
      right_wheel_tick_count.data--;  
    }   
  }
}
 
// Increment the number of ticks
void left_wheel_tick() {
   
  // Read the value for the encoder for the left wheel
  int val = digitalRead(ENC_IN_LEFT_B);
 
  if(val == LOW) {
    Direction_left = true; // Reverse
  }
  else {
    Direction_left = false; // Forward
  }
   
  if (Direction_left) {
    if (left_wheel_tick_count.data == encoder_maximum) {
      left_wheel_tick_count.data = encoder_minimum;
    }
    else {
      left_wheel_tick_count.data++;  
    }  
  }
  else {
    if (left_wheel_tick_count.data == encoder_minimum) {
      left_wheel_tick_count.data = encoder_maximum;
    }
    else {
      left_wheel_tick_count.data--;  
    }   
  }
}
 
void setup() {
 
  // Set pin states of the encoder
  pinMode(ENC_IN_LEFT_A , INPUT_PULLUP);
  pinMode(ENC_IN_LEFT_B , INPUT);
  pinMode(ENC_IN_RIGHT_A , INPUT_PULLUP);
  pinMode(ENC_IN_RIGHT_B , INPUT);
 
  // Every time the pin goes high, this is a tick
  attachInterrupt(digitalPinToInterrupt(ENC_IN_LEFT_A), left_wheel_tick, RISING);
  attachInterrupt(digitalPinToInterrupt(ENC_IN_RIGHT_A), right_wheel_tick, RISING);
 
  // ROS Setup
  nh.getHardware()->setBaud(115200);
  nh.initNode();
  nh.advertise(rightPub);
  nh.advertise(leftPub);
}
 
void loop() {
   
  // Record the time
  currentMillis = millis();
 
  // If 100ms have passed, print the number of ticks
  if (currentMillis - previousMillis > interval) {
     
    previousMillis = currentMillis;
     
    rightPub.publish( &right_wheel_tick_count );
    leftPub.publish( &left_wheel_tick_count );
    nh.spinOnce();
  }
}

• 이 코드는 왼쪽/오른쪽 모터의 tick counts를 right_ticks and left_ticks 이라는 이름의 ROS topic에 publish 함 

 

$ roscore

• 새로운 terminal 창을 연 후 master node 실행

$ rosrun rosserial_python serial_node.py _port:=/dev/ttyUSB0 _baud:=115200

• ROS serial server 실행
• Arduino와 연결된 USB port 확인 (이번 경우, ttyUSB0이었음)

$ rostopic list

• 활성화된 topics 확인

$ rostopic echo /right_ticks
$ rostopic echo /left_ticks

• 각 바퀴의 tick counts를 볼 수 있음

 

 

- Launch file 생성 (전에 진행했으나 remind를 위해 다시 작성)

$ cd ~/catkin_ws/src/kusmo_bot
$ catkin_create_pkg launch_kusmo_bot rospy roscpp std_msgs sensor_msgs geometry_msgs tf

• kusmo_bot directory로 이동 후 launch_kusmo_bot이라는 이름의 package 생성

$ cd launch_kusmo_bot
$ mkdir launch
cd ~/catkin_ws
catkin_make --only-pkg-with-deps  launch_kusmo_bot

• 생성한 package로 이동 후 launch directory 생성
• catkin workspace로 이동 후 package를 compile

$ roscd launch_kusmo_bot
$ gedit CMakeList.txt

• Package 내부로 이동 후 CMakeList.txt를 염
•  C++11 지원을 가능하게 하기 위해 5번째 줄의 hashtag('#')를 지움

$ cd launch
$ gedit kusmo_bot.launch

• launch folder로 이동 후 kusmo_bot.launch라는 이름의 새 launch file 생성
• 생성 후 rosserial, IMU, Lidar에 해당하는 내용들 입력

 

 

$ sudo chmod +x kusmo_bot.launch

• Launch file에 실행 권한 부여

$ cd
$ roslaunch launch_kusmo_bot kusmo_bot.launch

• Home directory로 이동 후 launch file 실행

$ rostopic list

• 활성화된 topics 확인

 

 

• 현재까지 'kusmo_bot.launch' file의 전체적인 내용은 아래와 같음

<?xml version="1.0"?>
<launch>
  <node pkg="rosserial_python" type="serial_node.py" name="serial_node">
    <param name="port" value="/dev/ttyACM0"/>
    <param name="baud" value="115200"/>
  </node> 
  
  <!-- IMU Data Publisher Using the iAHRS IMU Sensor -->
  <!-- Publish: /imu/data -->
  <param name="m_bSingle_TF_option" type="bool" value="true" /> <!--false-->
  <node pkg="iahrs_driver" type="iahrs_driver" name="iahrs_driver" output="screen" />

  
<!-- Lidar Data Publisher Using RPLIDAR from Slamtec -->
<!-- Used for obstacle avoidance and can be used for mapping -->
<!-- Publish: /scan -->
  <node name="rplidarNode"          pkg="rplidar_ros"  type="rplidarNode" output="screen">
  <param name="serial_port"         type="string" value="/dev/ttyUSB0"/>
  <param name="serial_baudrate"     type="int"    value="115200"/><!--A1/A2 -->
  <!--param name="serial_baudrate"     type="int"    value="256000"--><!--A3 -->
  <param name="frame_id"            type="string" value="laser"/>
  <param name="inverted"            type="bool"   value="false"/>
  <param name="angle_compensate"    type="bool"   value="true"/>
  </node> 
</launch>

 

 

 

 

 

 

---

https://automaticaddison.com/how-to-publish-wheel-encoder-tick-data-using-ros-and-arduino/

How to Publish Wheel Encoder Tick Data Using ROS and Arduino – Automatic Addison

https://automaticaddison.com/how-to-publish-imu-data-using-ros-and-the-bno055-imu-sensor/

How to Publish IMU Data Using ROS and the BNO055 IMU Sensor – Automatic Addison