STM32 ARM에서 ROS와 연동하기(1, publisher)

 

ROS(Robot OS System)

ROS는 rosserial 를 통해서  외부 microcontroller와 통신이 가능하다.

rosserial은 시리얼 통신으로 받은 데이터를 메세지 타입으로 변환하여 Node 간 메시지를 전송할 수 있게 한다.

따라서 microcontroller와 메시지를 주고 받기 위해서는 Master PC에 rosserial가 준비되어야 한다.

 

Mbed를 사용하면 ROS와 연동할 수 있는 라이브러리를 사용할 수 있다.

물론 ROS 버전에 맞춰 사용해야 하며, 이 라이브러리를 사용하게 되면 ROS에서 사용한 코드와 동일하게  publisher와 subscriber, message를 만들어서 사용할 수 있다. ARM 계열 사용자는 Mbed를 통해서 쉽게 ROS와 연결 할 수 있어서 편리하다.

 

 

ROS kinetic 버전 라이브러리

 

Mbed Compiler 환경에서 Import - "kinetic" 검색하면  라이브러리들이 나타나는데, 가장 많이 다운받은 것이

Gary Servin이 만든 것이라고 나타날 것이다. 이것을 다운받은 후 사용하면 된다.

 

아래 코드는 모터 엔코더(absolute encoder) 2개로부터 각도 값을 받아서 메시지를 보내는 퍼블리셔이다.

사용한 엔코더는 SSI 통신을 한다고 되어 있는데 SPI와 유사하다. 단지 MOSI 가 없다.

 

2가지의 데이터를 각각 보내기 위해  Encoder1_pub, Encoder2_pub을 만든다.그리고

Encoder1 과 Encoder2라는 Topic을 만들어서 그 안에 각각 msgEncoder1, msgEncoder2라는 메세지를 담았다.

 

publisher 생성

 

엔코더로부터 받은 각도값(Angle_encoder x)를 각각의 메세지에 담은 후 퍼블리쉬하는 Mesage_Publisher()

Message_Publisher()

 

 

main에서 ROS를 init한 뒤, 2개의 퍼블리셔를 advertise로 통해 퍼블리쉬한다. 통신 속도는 57600이 한계인 듯하다.

 

 

100ms마다 엔코더 값을 읽어서 메세지를 Master PC로 전달한다.

동작

[ Code ]

#include "mbed.h"
#include <ros.h>
#include <std_msgs/Float32.h>
////////////////////////////////////////////////////////////////////////////////
////////////////////////////////////////////////////////////////////////////////
Serial pc(USBTX, USBRX); 
Timer timer1;
 
////////////////////////////////////////////////////////////////////////////////
//******  SPI spiX(mosi, miso, sclk)  ****//
// Encoder 1 ///////////////////////////////
SPI spi6(PG_14, PG_12, PG_13);
DigitalOut CS1(PG_11);
 
// Encoder 2 ///////////////////////////////
SPI spi5(PF_9, PF_8, PF_7);
DigitalOut CS2(PF_6);
 
 
////////////////////////////////////////////////////////////////////////////////
uint8_t  Hbyte_enc1 = 0;
uint8_t  Lbyte_enc1 = 0;
float    Angle_encoder1 = 0;
uint8_t  Hbyte_enc2 = 0;
uint8_t  Lbyte_enc2 = 0;
float    Angle_encoder2 = 0;
 
 
////////////////////////////////////////////////////////////////////////////////
////////////////////////////////////////////////////////////////////////////////
ros::NodeHandle nh;
std_msgs::Float32 msgEncoder1;
std_msgs::Float32 msgEncoder2;
 
ros::Publisher Encoder1_pub("Encoder1", &msgEncoder1);
ros::Publisher Encoder2_pub("Encoder2", &msgEncoder2);
 
 
////////////////////////////////////////////////////////////////////////////////
////////////////////////////////////////////////////////////////////////////////
void SetSPI(SPI&spi)
{
  spi.format(8, 1);
  spi.frequency(500000); //500 kHz    
}
 
void ReadEncoder(SPI&spi, uint8_t &low, uint8_t &high, DigitalOut& CS, float &angle)
{
  CS = 0;
  high = spi.write(0x00);
  low = spi.write(0x00);
  CS = 1;
  
  uint16_t Data = ((uint16_t)high << 2) + ((uint16_t)low >>6); 
  angle = (float)Data * (360.0 / 1024.0);
  
}
 
void Message_Publisher()
{
  msgEncoder1.data = Angle_encoder1;
  Encoder1_pub.publish(&msgEncoder1);
 
  msgEncoder2.data = Angle_encoder2;
  Encoder2_pub.publish(&msgEncoder2);
 
}
 
////////////////////////////////////////////////////////////////////////////////
int main()
{ 
  //SPI 
  SetSPI(spi1); 
  SetSPI(spi2);
 
  
  //ROS
  nh.initNode();
  nh.advertise(Encoder1_pub);
  nh.advertise(Encoder2_pub);
  nh.getHardware()->setBaud(57600);
  
  timer1.start();
  
    while(1) {
      
      if(timer1.read_ms()>100)
      {
        ReadEncoder(spi6, Lbyte_enc1, Hbyte_enc1, CS1, Angle_encoder1);
        ReadEncoder(spi5, Lbyte_enc2, Hbyte_enc2, CS2, Angle_encoder2);    
        
        Message_Publisher();
        timer1.reset();
      }
      
     nh.spinOnce();
     // pc.printf("%.1f\n", Angle_encoder1);
     // pc.printf("%.1f\n", Angle_encoder2);
     
     
    }
}