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