STM32F429i-discoでrosserial

STM32F429i-discoはmbedに対応しているので、rosserial_mbedをインポートしてROSにつなげてみました。

mbed IDEでrosserial_mbedをインポートして、ローカルのSW4STM32で開発しています。

joyトピックをsubscribeするコードを書いてみます。

肝心のSTM側のシリアルポートは、デフォルトでUSART1(PA9,PA10), 57600bpsで初期化されます。
このコードではボーレートを921600に変更しています。sensor_msgs::Joy型のサイズが意外と大きく、57600bpsだと遅延がかなり感じられます。

  ros::NodeHandle nh;
  ros::Subscriber<sensor_msgs::Joy> sub("joy", &joy_callback);
  nh.getHardware()->setBaud(921600);
  nh.initNode();
  nh.subscribe(sub);

  while(1) {
    nh.spinOnce();
  }

トピックが送られてきた時に呼び出されるcallback関数は、とりあえずボタンの情報をLCDに表示するものにしました。

void joy_callback(const sensor_msgs::Joy &joy)
{
	int button_size = joy.buttons_length;
	char time_stamp[128];
	for(int i=0; i<button_size; i++) {
		if(joy.buttons[i]) {
			lcd.DisplayStringAt(lcd.GetFont()->Width * i, LINE(3), (uint8_t *)"1", LEFT_MODE);
		}
		else {
			lcd.DisplayStringAt(lcd.GetFont()->Width * i, LINE(3), (uint8_t *)"0", LEFT_MODE);
		}
	}
	sprintf(time_stamp, "%d", (int)joy.header.stamp.sec);
	lcd.DisplayStringAt(0, LINE(4), (uint8_t *)time_stamp, LEFT_MODE);

}

ROS側では、roscoreを起動してからまずrosserialノードを起動します。デフォルトのポートはttyUSB0です。

rosrun rosserial_python serial_node.py _baud:=921600

接続されると、STMのsubscriberの情報が表示されます。

[INFO] [1485587675.953141]: ROS Serial Python Node
[INFO] [1485587675.968201]: Connecting to /dev/ttyUSB0 at 921600 baud
[INFO] [1485587678.111717]: Note: subscribe buffer size is 512 bytes
[INFO] [1485587678.112621]: Setup subscriber on joy [sensor_msgs/Joy]

あとはDualshock3などジョイスティックデバイスをPCに接続し、joyノードを起動すればSTM側にsensor_msgs::Joy型のトピックが届きます！

rosrun joy joy_node